update code

160.yaml

@@ -0,0 +1,7 @@
+image: 160_160.png
+resolution: 0.050000
+origin: [-10.000000, -10.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

obj_dec/.catkin_workspace

@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration

obj_dec/.vscode/c_cpp_properties.json

@@ -0,0 +1,20 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**",
+                "/opt/ros/noetic/include/**",
+                "/opt/ros/noetic/include",
+                "${workspaceFolder}/src/rknn_yolov5_demo/include",
+                "${workspaceFolder}/devel/include"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c17",
+            "cppStandard": "gnu++14",
+            "intelliSenseMode": "linux-gcc-arm64"
+        }
+    ],
+    "version": 4
+}

obj_dec/.vscode/settings.json

@@ -0,0 +1,80 @@
+{
+    "C_Cpp.errorSquiggles": "disabled",
+    "files.associations": {
+        "deque": "cpp",
+        "string": "cpp",
+        "vector": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "any": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "strstream": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "codecvt": "cpp",
+        "complex": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "map": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "set": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "shared_mutex": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cfenv": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "valarray": "cpp",
+        "variant": "cpp",
+        "bit": "cpp"
+    },
+    "remote.SSH.allowX11Forwarding": true,
+
+    "cmake.sourceDirectory": "/home/firefly/obj_dec/src/rknn_yolov5_demo"
+}

obj_dec/src/3rdparty/rga/RK3588/include/GrallocOps.h

@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) 2016 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  Zhiqin Wei <wzq@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef _rk_graphic_buffer_h_
+#define _rk_graphic_buffer_h_
+
+#ifdef ANDROID
+
+#include <stdint.h>
+#include <vector>
+#include <sys/types.h>
+
+#include <system/graphics.h>
+
+#include <utils/Thread.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <time.h>
+#include <unistd.h>
+
+#include <sys/mman.h>
+#include <linux/stddef.h>
+
+#include <utils/Atomic.h>
+#include <utils/Errors.h>
+#include <android/log.h>
+#include <utils/Log.h>
+#include <log/log_main.h>
+
+#include "drmrga.h"
+#include "rga.h"
+
+// -------------------------------------------------------------------------------
+int         RkRgaGetHandleFd(buffer_handle_t handle, int *fd);
+int         RkRgaGetHandleAttributes(buffer_handle_t handle,
+                                     std::vector<int> *attrs);
+int         RkRgaGetHandleMapAddress(buffer_handle_t handle,
+                                     void **buf);
+#endif  //Android
+
+#endif  //_rk_graphic_buffer_h_

obj_dec/src/3rdparty/rga/RK3588/include/RgaApi.h

@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2016 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *    Zhiqin Wei <wzq@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef _rockchip_rga_c_h_
+#define _rockchip_rga_c_h_
+
+#include <stdint.h>
+#include <sys/types.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <time.h>
+#include <unistd.h>
+
+#include <sys/mman.h>
+#include <linux/stddef.h>
+
+#include "drmrga.h"
+#include "rga.h"
+
+#ifdef __cplusplus
+extern "C"{
+#endif
+
+/*
+ * Compatible with the old version of C interface.The new
+ * version of the C interface no longer requires users to
+ * initialize rga, so RgaInit and RgaDeInit are just for
+ * compatibility with the old C interface, so please do
+ * not use ctx, because it is usually a NULL.
+ */
+#define RgaInit(ctx) ({ \
+    int ret = 0; \
+    ret = c_RkRgaInit(); \
+    c_RkRgaGetContext(ctx); \
+    ret;\
+})
+#define RgaDeInit(ctx) { \
+    (void)ctx;        /* unused */ \
+    c_RkRgaDeInit(); \
+}
+#define RgaBlit(...) c_RkRgaBlit(__VA_ARGS__)
+#define RgaCollorFill(...) c_RkRgaColorFill(__VA_ARGS__)
+#define RgaFlush() c_RkRgaFlush()
+
+int  c_RkRgaInit();
+void c_RkRgaDeInit();
+void c_RkRgaGetContext(void **ctx);
+int  c_RkRgaBlit(rga_info_t *src, rga_info_t *dst, rga_info_t *src1);
+int  c_RkRgaColorFill(rga_info_t *dst);
+int  c_RkRgaFlush();
+
+#ifndef ANDROID /* linux */
+int c_RkRgaGetAllocBuffer(bo_t *bo_info, int width, int height, int bpp);
+int c_RkRgaGetAllocBufferCache(bo_t *bo_info, int width, int height, int bpp);
+int c_RkRgaGetMmap(bo_t *bo_info);
+int c_RkRgaUnmap(bo_t *bo_info);
+int c_RkRgaFree(bo_t *bo_info);
+int c_RkRgaGetBufferFd(bo_t *bo_info, int *fd);
+#endif /* #ifndef ANDROID */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* #ifndef _rockchip_rga_c_h_ */

obj_dec/src/3rdparty/rga/RK3588/include/RgaMutex.h

@@ -0,0 +1,193 @@
+/*
+ * Copyright (C) 2020 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  PutinLee <putin.lee@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#ifndef _LIBS_RGA_MUTEX_H
+#define _LIBS_RGA_MUTEX_H
+
+#ifndef ANDROID
+#include <stdint.h>
+#include <sys/types.h>
+#include <time.h>
+
+#include <pthread.h>
+
+
+// Enable thread safety attributes only with clang.
+// The attributes can be safely erased when compiling with other compilers.
+#if defined(__clang__) && (!defined(SWIG))
+#define THREAD_ANNOTATION_ATTRIBUTE__(x) __attribute__((x))
+#else
+#define THREAD_ANNOTATION_ATTRIBUTE__(x)  // no-op
+#endif
+
+#define CAPABILITY(x) THREAD_ANNOTATION_ATTRIBUTE__(capability(x))
+
+#define SCOPED_CAPABILITY THREAD_ANNOTATION_ATTRIBUTE__(scoped_lockable)
+
+#define GUARDED_BY(x) THREAD_ANNOTATION_ATTRIBUTE__(guarded_by(x))
+
+#define PT_GUARDED_BY(x) THREAD_ANNOTATION_ATTRIBUTE__(pt_guarded_by(x))
+
+#define ACQUIRED_BEFORE(...) THREAD_ANNOTATION_ATTRIBUTE__(acquired_before(__VA_ARGS__))
+
+#define ACQUIRED_AFTER(...) THREAD_ANNOTATION_ATTRIBUTE__(acquired_after(__VA_ARGS__))
+
+#define REQUIRES(...) THREAD_ANNOTATION_ATTRIBUTE__(requires_capability(__VA_ARGS__))
+
+#define REQUIRES_SHARED(...) THREAD_ANNOTATION_ATTRIBUTE__(requires_shared_capability(__VA_ARGS__))
+
+#define ACQUIRE(...) THREAD_ANNOTATION_ATTRIBUTE__(acquire_capability(__VA_ARGS__))
+
+#define ACQUIRE_SHARED(...) THREAD_ANNOTATION_ATTRIBUTE__(acquire_shared_capability(__VA_ARGS__))
+
+#define RELEASE(...) THREAD_ANNOTATION_ATTRIBUTE__(release_capability(__VA_ARGS__))
+
+#define RELEASE_SHARED(...) THREAD_ANNOTATION_ATTRIBUTE__(release_shared_capability(__VA_ARGS__))
+
+#define TRY_ACQUIRE(...) THREAD_ANNOTATION_ATTRIBUTE__(try_acquire_capability(__VA_ARGS__))
+
+#define TRY_ACQUIRE_SHARED(...) \
+    THREAD_ANNOTATION_ATTRIBUTE__(try_acquire_shared_capability(__VA_ARGS__))
+
+#define EXCLUDES(...) THREAD_ANNOTATION_ATTRIBUTE__(locks_excluded(__VA_ARGS__))
+
+#define ASSERT_CAPABILITY(x) THREAD_ANNOTATION_ATTRIBUTE__(assert_capability(x))
+
+#define ASSERT_SHARED_CAPABILITY(x) THREAD_ANNOTATION_ATTRIBUTE__(assert_shared_capability(x))
+
+#define RETURN_CAPABILITY(x) THREAD_ANNOTATION_ATTRIBUTE__(lock_returned(x))
+
+#define NO_THREAD_SAFETY_ANALYSIS THREAD_ANNOTATION_ATTRIBUTE__(no_thread_safety_analysis)
+
+class Condition;
+
+/*
+ * NOTE: This class is for code that builds on Win32.  Its usage is
+ * deprecated for code which doesn't build for Win32.  New code which
+ * doesn't build for Win32 should use std::mutex and std::lock_guard instead.
+ *
+ * Simple mutex class.  The implementation is system-dependent.
+ *
+ * The mutex must be unlocked by the thread that locked it.  They are not
+ * recursive, i.e. the same thread can't lock it multiple times.
+ */
+class CAPABILITY("mutex") Mutex {
+  public:
+    enum {
+        PRIVATE = 0,
+        SHARED = 1
+    };
+
+    Mutex();
+    explicit Mutex(const char* name);
+    explicit Mutex(int type, const char* name = nullptr);
+    ~Mutex();
+
+    // lock or unlock the mutex
+    int32_t lock() ACQUIRE();
+    void unlock() RELEASE();
+
+    // lock if possible; returns 0 on success, error otherwise
+    int32_t tryLock() TRY_ACQUIRE(0);
+
+    int32_t timedLock(int64_t timeoutNs) TRY_ACQUIRE(0);
+
+    // Manages the mutex automatically. It'll be locked when Autolock is
+    // constructed and released when Autolock goes out of scope.
+    class SCOPED_CAPABILITY Autolock {
+      public:
+        inline explicit Autolock(Mutex& mutex) ACQUIRE(mutex) : mLock(mutex) {
+            mLock.lock();
+        }
+        inline explicit Autolock(Mutex* mutex) ACQUIRE(mutex) : mLock(*mutex) {
+            mLock.lock();
+        }
+        inline ~Autolock() RELEASE() {
+            mLock.unlock();
+        }
+
+      private:
+        Mutex& mLock;
+        // Cannot be copied or moved - declarations only
+        Autolock(const Autolock&);
+        Autolock& operator=(const Autolock&);
+    };
+
+  private:
+    friend class Condition;
+
+    // A mutex cannot be copied
+    Mutex(const Mutex&);
+    Mutex& operator=(const Mutex&);
+
+    pthread_mutex_t mMutex;
+};
+
+// ---------------------------------------------------------------------------
+inline Mutex::Mutex() {
+    pthread_mutex_init(&mMutex, nullptr);
+}
+inline Mutex::Mutex(__attribute__((unused)) const char* name) {
+    pthread_mutex_init(&mMutex, nullptr);
+}
+inline Mutex::Mutex(int type, __attribute__((unused)) const char* name) {
+    if (type == SHARED) {
+        pthread_mutexattr_t attr;
+        pthread_mutexattr_init(&attr);
+        pthread_mutexattr_setpshared(&attr, PTHREAD_PROCESS_SHARED);
+        pthread_mutex_init(&mMutex, &attr);
+        pthread_mutexattr_destroy(&attr);
+    } else {
+        pthread_mutex_init(&mMutex, nullptr);
+    }
+}
+inline Mutex::~Mutex() {
+    pthread_mutex_destroy(&mMutex);
+}
+inline int32_t Mutex::lock() {
+    return -pthread_mutex_lock(&mMutex);
+}
+inline void Mutex::unlock() {
+    pthread_mutex_unlock(&mMutex);
+}
+inline int32_t Mutex::tryLock() {
+    return -pthread_mutex_trylock(&mMutex);
+}
+inline int32_t Mutex::timedLock(int64_t timeoutNs) {
+    timespec now;
+    clock_gettime(CLOCK_REALTIME, &now);
+    timeoutNs += now.tv_sec*1000000000 + now.tv_nsec;
+    const struct timespec ts = {
+        /* .tv_sec = */ static_cast<time_t>(timeoutNs / 1000000000),
+        /* .tv_nsec = */ static_cast<long>(timeoutNs % 1000000000),
+    };
+    return -pthread_mutex_timedlock(&mMutex, &ts);
+}
+
+// ---------------------------------------------------------------------------
+
+/*
+ * Automatic mutex.  Declare one of these at the top of a function.
+ * When the function returns, it will go out of scope, and release the
+ * mutex.
+ */
+
+typedef Mutex::Autolock AutoMutex;
+#endif // __ANDROID_VNDK__
+#endif // _LIBS_RGA_MUTEX_H

obj_dec/src/3rdparty/rga/RK3588/include/RgaSingleton.h

@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) 2016 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *    Zhiqin Wei <wzq@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+ 
+#ifndef _LIBS_RGA_SINGLETON_H
+#define _LIBS_RGA_SINGLETON_H
+
+#ifndef ANDROID
+#include "RgaMutex.h"
+
+#if defined(__clang__)
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wundefined-var-template"
+#endif
+
+template <typename TYPE>
+class Singleton {
+  public:
+    static TYPE& getInstance() {
+        Mutex::Autolock _l(sLock);
+        TYPE* instance = sInstance;
+        if (instance == nullptr) {
+            instance = new TYPE();
+            sInstance = instance;
+        }
+        return *instance;
+    }
+
+    static bool hasInstance() {
+        Mutex::Autolock _l(sLock);
+        return sInstance != nullptr;
+    }
+
+  protected:
+    ~Singleton() { }
+    Singleton() { }
+
+  private:
+    Singleton(const Singleton&);
+    Singleton& operator = (const Singleton&);
+    static Mutex sLock;
+    static TYPE* sInstance;
+};
+
+#if defined(__clang__)
+#pragma clang diagnostic pop
+#endif
+
+#define RGA_SINGLETON_STATIC_INSTANCE(TYPE)                 \
+    template<> ::Mutex  \
+        (::Singleton< TYPE >::sLock)(::Mutex::PRIVATE);  \
+    template<> TYPE* ::Singleton< TYPE >::sInstance(nullptr);  /* NOLINT */ \
+    template class ::Singleton< TYPE >;
+
+#endif //ANDROID
+#endif //_LIBS_RGA_SINGLETON_H

obj_dec/src/3rdparty/rga/RK3588/include/RgaUtils.h

@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2016 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  Zhiqin Wei <wzq@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef _rga_utils_h_
+#define _rga_utils_h_
+
+// -------------------------------------------------------------------------------
+float get_bpp_from_format(int format);
+int get_perPixel_stride_from_format(int format);
+int get_buf_from_file(void *buf, int f, int sw, int sh, int index);
+int output_buf_data_to_file(void *buf, int f, int sw, int sh, int index);
+const char *translate_format_str(int format);
+int get_buf_from_file_FBC(void *buf, int f, int sw, int sh, int index);
+int output_buf_data_to_file_FBC(void *buf, int f, int sw, int sh, int index);
+#endif
+

obj_dec/src/3rdparty/rga/RK3588/include/RockchipRga.h

@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2016 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  Zhiqin Wei <wzq@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef _rockchip_rga_h_
+#define _rockchip_rga_h_
+
+#include <stdint.h>
+#include <sys/types.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <time.h>
+#include <unistd.h>
+#include <sys/mman.h>
+#include <linux/stddef.h>
+
+#include "drmrga.h"
+#include "GrallocOps.h"
+#include "RgaUtils.h"
+#include "rga.h"
+
+//////////////////////////////////////////////////////////////////////////////////
+#ifndef ANDROID
+#include "RgaSingleton.h"
+#endif
+
+#ifdef ANDROID
+#include <utils/Singleton.h>
+#include <utils/Thread.h>
+#include <hardware/hardware.h>
+
+namespace android {
+#endif
+
+    class RockchipRga :public Singleton<RockchipRga> {
+      public:
+
+        static inline RockchipRga& get() {
+            return getInstance();
+        }
+
+        int         RkRgaInit();
+        void        RkRgaDeInit();
+        void        RkRgaGetContext(void **ctx);
+#ifndef ANDROID /* LINUX */
+        int         RkRgaAllocBuffer(int drm_fd /* input */, bo_t *bo_info,
+                                     int width, int height, int bpp, int flags);
+        int         RkRgaFreeBuffer(int drm_fd /* input */, bo_t *bo_info);
+        int         RkRgaGetAllocBuffer(bo_t *bo_info, int width, int height, int bpp);
+        int         RkRgaGetAllocBufferExt(bo_t *bo_info, int width, int height, int bpp, int flags);
+        int         RkRgaGetAllocBufferCache(bo_t *bo_info, int width, int height, int bpp);
+        int         RkRgaGetMmap(bo_t *bo_info);
+        int         RkRgaUnmap(bo_t *bo_info);
+        int         RkRgaFree(bo_t *bo_info);
+        int         RkRgaGetBufferFd(bo_t *bo_info, int *fd);
+#else
+        int         RkRgaGetBufferFd(buffer_handle_t handle, int *fd);
+        int         RkRgaGetHandleMapCpuAddress(buffer_handle_t handle, void **buf);
+#endif
+        int         RkRgaBlit(rga_info *src, rga_info *dst, rga_info *src1);
+        int         RkRgaCollorFill(rga_info *dst);
+        int         RkRgaCollorPalette(rga_info *src, rga_info *dst, rga_info *lut);
+        int         RkRgaFlush();
+
+
+        void        RkRgaSetLogOnceFlag(int log) {
+            mLogOnce = log;
+        }
+        void        RkRgaSetAlwaysLogFlag(bool log) {
+            mLogAlways = log;
+        }
+        void        RkRgaLogOutRgaReq(struct rga_req rgaReg);
+        int         RkRgaLogOutUserPara(rga_info *rgaInfo);
+        inline bool RkRgaIsReady() {
+            return mSupportRga;
+        }
+
+        RockchipRga();
+        ~RockchipRga();
+      private:
+        bool                            mSupportRga;
+        int                             mLogOnce;
+        int                             mLogAlways;
+        void *                          mContext;
+
+        friend class Singleton<RockchipRga>;
+    };
+
+#ifdef ANDROID
+}; // namespace android
+#endif
+
+#endif
+

obj_dec/src/3rdparty/rga/RK3588/include/drmrga.h

@@ -0,0 +1,358 @@
+/*
+ * Copyright (C) 2016 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  Zhiqin Wei <wzq@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef _rk_drm_rga_
+#define _rk_drm_rga_
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+
+#include "rga.h"
+
+#ifdef ANDROID
+#define DRMRGA_HARDWARE_MODULE_ID "librga"
+
+#include <hardware/gralloc.h>
+#include <hardware/hardware.h>
+#include <system/graphics.h>
+#include <cutils/native_handle.h>
+
+#ifdef ANDROID_12
+#include <hardware/hardware_rockchip.h>
+#endif
+
+#endif
+
+#define RGA_BLIT_SYNC   0x5017
+#define RGA_BLIT_ASYNC  0x5018
+
+#ifndef ANDROID /* LINUX */
+/* flip source image horizontally (around the vertical axis) */
+#define HAL_TRANSFORM_FLIP_H     0x01
+/* flip source image vertically (around the horizontal axis)*/
+#define HAL_TRANSFORM_FLIP_V     0x02
+/* rotate source image 90 degrees clockwise */
+#define HAL_TRANSFORM_ROT_90     0x04
+/* rotate source image 180 degrees */
+#define HAL_TRANSFORM_ROT_180    0x03
+/* rotate source image 270 degrees clockwise */
+#define HAL_TRANSFORM_ROT_270    0x07
+#endif
+
+#define HAL_TRANSFORM_FLIP_H_V   0x08
+
+/*****************************************************************************/
+
+/* for compatibility */
+#define DRM_RGA_MODULE_API_VERSION      HWC_MODULE_API_VERSION_0_1
+#define DRM_RGA_DEVICE_API_VERSION      HWC_DEVICE_API_VERSION_0_1
+#define DRM_RGA_API_VERSION             HWC_DEVICE_API_VERSION
+
+#define DRM_RGA_TRANSFORM_ROT_MASK      0x0000000F
+#define DRM_RGA_TRANSFORM_ROT_0         0x00000000
+#define DRM_RGA_TRANSFORM_ROT_90        HAL_TRANSFORM_ROT_90
+#define DRM_RGA_TRANSFORM_ROT_180       HAL_TRANSFORM_ROT_180
+#define DRM_RGA_TRANSFORM_ROT_270       HAL_TRANSFORM_ROT_270
+
+#define DRM_RGA_TRANSFORM_FLIP_MASK     0x00000003
+#define DRM_RGA_TRANSFORM_FLIP_H        HAL_TRANSFORM_FLIP_H
+#define DRM_RGA_TRANSFORM_FLIP_V        HAL_TRANSFORM_FLIP_V
+
+enum {
+    AWIDTH                      = 0,
+    AHEIGHT,
+    ASTRIDE,
+    AFORMAT,
+    ASIZE,
+    ATYPE,
+};
+/*****************************************************************************/
+
+#ifndef ANDROID /* LINUX */
+/* memory type definitions. */
+enum drm_rockchip_gem_mem_type {
+    /* Physically Continuous memory and used as default. */
+    ROCKCHIP_BO_CONTIG  = 1 << 0,
+    /* cachable mapping. */
+    ROCKCHIP_BO_CACHABLE    = 1 << 1,
+    /* write-combine mapping. */
+    ROCKCHIP_BO_WC      = 1 << 2,
+    ROCKCHIP_BO_SECURE  = 1 << 3,
+    ROCKCHIP_BO_MASK    = ROCKCHIP_BO_CONTIG | ROCKCHIP_BO_CACHABLE |
+                ROCKCHIP_BO_WC | ROCKCHIP_BO_SECURE
+};
+
+typedef struct bo {
+    int fd;
+    void *ptr;
+    size_t size;
+    size_t offset;
+    size_t pitch;
+    unsigned handle;
+} bo_t;
+#endif
+
+/*
+   @value size:     user not need care about.For avoid read/write out of memory
+ */
+typedef struct rga_rect {
+    int xoffset;
+    int yoffset;
+    int width;
+    int height;
+    int wstride;
+    int hstride;
+    int format;
+    int size;
+} rga_rect_t;
+
+typedef struct rga_nn {
+    int nn_flag;
+    int scale_r;
+    int scale_g;
+    int scale_b;
+    int offset_r;
+    int offset_g;
+    int offset_b;
+} rga_nn_t;
+
+typedef struct rga_dither {
+    int enable;
+    int mode;
+    int lut0_l;
+    int lut0_h;
+    int lut1_l;
+    int lut1_h;
+} rga_dither_t;
+
+struct rga_mosaic_info {
+    uint8_t enable;
+    uint8_t mode;
+};
+
+struct rga_pre_intr_info {
+    uint8_t enable;
+
+    uint8_t read_intr_en;
+    uint8_t write_intr_en;
+    uint8_t read_hold_en;
+    uint32_t read_threshold;
+    uint32_t write_start;
+    uint32_t write_step;
+};
+
+/* MAX(min, (max - channel_value)) */
+struct rga_osd_invert_factor {
+    uint8_t alpha_max;
+    uint8_t alpha_min;
+    uint8_t yg_max;
+    uint8_t yg_min;
+    uint8_t crb_max;
+    uint8_t crb_min;
+};
+
+struct rga_color {
+    union {
+        struct {
+            uint8_t red;
+            uint8_t green;
+            uint8_t blue;
+            uint8_t alpha;
+        };
+        uint32_t value;
+    };
+};
+
+struct rga_osd_bpp2 {
+    uint8_t  ac_swap;           // ac swap flag
+                                // 0: CA
+                                // 1: AC
+    uint8_t  endian_swap;       // rgba2bpp endian swap
+                                // 0: Big endian
+                                // 1: Little endian
+    struct rga_color color0;
+    struct rga_color color1;
+};
+
+struct rga_osd_mode_ctrl {
+    uint8_t mode;               // OSD cal mode:
+                                //   0b'1: statistics mode
+                                //   1b'1: auto inversion overlap mode
+    uint8_t direction_mode;     // horizontal or vertical
+                                //   0: horizontal
+                                //   1: vertical
+    uint8_t width_mode;         // using @fix_width or LUT width
+                                //   0: fix width
+                                //   1: LUT width
+    uint16_t block_fix_width;   // OSD block fixed width
+                                //   real width = (fix_width + 1) * 2
+    uint8_t block_num;          // OSD block num
+    uint16_t flags_index;       // auto invert flags index
+
+    /* invertion config */
+    uint8_t color_mode;         // selete color
+                                //   0: src1 color
+                                //   1: config data color
+    uint8_t invert_flags_mode;  // invert flag selete
+                                //   0: use RAM flag
+                                //   1: usr last result
+    uint8_t default_color_sel;  // default color mode
+                                //   0: default is bright
+                                //   1: default is dark
+    uint8_t invert_enable;      // invert channel enable
+                                //   1 << 0: aplha enable
+                                //   1 << 1: Y/G disable
+                                //   1 << 2: C/RB disable
+    uint8_t invert_mode;        // invert cal mode
+                                //   0: normal(max-data)
+                                //   1: swap
+    uint8_t invert_thresh;      // if luma > thresh, osd_flag to be 1
+    uint8_t unfix_index;        // OSD width config index
+};
+
+struct rga_osd_info {
+    uint8_t  enable;
+
+    struct rga_osd_mode_ctrl mode_ctrl;
+    struct rga_osd_invert_factor cal_factor;
+    struct rga_osd_bpp2 bpp2_info;
+
+    union {
+        struct {
+            uint32_t last_flags1;
+            uint32_t last_flags0;
+        };
+        uint64_t last_flags;
+    };
+
+    union {
+        struct {
+            uint32_t cur_flags1;
+            uint32_t cur_flags0;
+        };
+        uint64_t cur_flags;
+    };
+};
+
+/*
+   @value fd:     use fd to share memory, it can be ion shard fd,and dma fd.
+   @value virAddr:userspace address
+   @value phyAddr:use phy address
+   @value hnd:    use buffer_handle_t
+ */
+typedef struct rga_info {
+    int fd;
+    void *virAddr;
+    void *phyAddr;
+#ifndef ANDROID /* LINUX */
+    unsigned hnd;
+#else /* Android */
+    buffer_handle_t hnd;
+#endif
+    int format;
+    rga_rect_t rect;
+    unsigned int blend;
+    int bufferSize;
+    int rotation;
+    int color;
+    int testLog;
+    int mmuFlag;
+    int colorkey_en;
+    int colorkey_mode;
+    int colorkey_max;
+    int colorkey_min;
+    int scale_mode;
+    int color_space_mode;
+    int sync_mode;
+    rga_nn_t nn;
+    rga_dither_t dither;
+    int rop_code;
+    int rd_mode;
+    unsigned short is_10b_compact;
+    unsigned short is_10b_endian;
+
+    int in_fence_fd;
+    int out_fence_fd;
+
+    int core;
+    int priority;
+
+    unsigned short enable;
+
+    int handle;
+
+    struct rga_mosaic_info mosaic_info;
+
+    struct rga_osd_info osd_info;
+
+    struct rga_pre_intr_info pre_intr;
+
+    int mpi_mode;
+
+    union {
+        int ctx_id;
+        int job_handle;
+    };
+
+    char reserve[402];
+} rga_info_t;
+
+
+typedef struct drm_rga {
+    rga_rect_t src;
+    rga_rect_t dst;
+} drm_rga_t;
+
+/*
+   @fun rga_set_rect:For use to set the rects esayly
+
+   @param rect:The rect user want to set,like setting the src rect:
+   drm_rga_t rects;
+   rga_set_rect(rects.src,0,0,1920,1080,1920,NV12);
+   mean to set the src rect to the value.
+ */
+static inline int rga_set_rect(rga_rect_t *rect,
+                               int x, int y, int w, int h, int sw, int sh, int f) {
+    if (!rect)
+        return -EINVAL;
+
+    rect->xoffset = x;
+    rect->yoffset = y;
+    rect->width = w;
+    rect->height = h;
+    rect->wstride = sw;
+    rect->hstride = sh;
+    rect->format = f;
+
+    return 0;
+}
+
+#ifndef ANDROID /* LINUX */
+static inline void rga_set_rotation(rga_info_t *info, int angle) {
+    if (angle == 90)
+        info->rotation = HAL_TRANSFORM_ROT_90;
+    else if (angle == 180)
+        info->rotation = HAL_TRANSFORM_ROT_180;
+    else if (angle == 270)
+        info->rotation = HAL_TRANSFORM_ROT_270;
+}
+#endif
+/*****************************************************************************/
+
+#endif

obj_dec/src/3rdparty/rga/RK3588/include/im2d.h

@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2020 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  PutinLee <putin.lee@rock-chips.com>
+ *  Cerf Yu <cerf.yu@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef _im2d_h_
+#define _im2d_h_
+
+#include "im2d_version.h"
+#include "im2d_type.h"
+
+#include "im2d_common.h"
+#include "im2d_buffer.h"
+#include "im2d_single.h"
+#include "im2d_task.h"
+#include "im2d_mpi.h"
+
+#endif /* #ifndef _im2d_h_ */

obj_dec/src/3rdparty/rga/RK3588/include/im2d.hpp

@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2020 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  PutinLee <putin.lee@rock-chips.com>
+ *  Cerf Yu <cerf.yu@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef _im2d_hpp_
+#define _im2d_hpp_
+
+#include "im2d.h"
+#include "im2d_expand.h"
+
+#endif /* #ifndef _im2d_hpp_ */
+
+

obj_dec/src/3rdparty/rga/RK3588/include/im2d_buffer.h

@@ -0,0 +1,179 @@
+/*
+ * Copyright (C) 2022 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  Cerf Yu <cerf.yu@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef _im2d_buffer_h_
+#define _im2d_buffer_h_
+
+#include "im2d_type.h"
+
+/**
+ * Import external buffers into RGA driver.
+ *
+ * @param fd/va/pa
+ *      Select dma_fd/virtual_address/physical_address by buffer type
+ * @param size
+ *      Describes the size of the image buffer
+ *
+ * @return rga_buffer_handle_t
+ */
+#ifdef __cplusplus
+IM_API rga_buffer_handle_t importbuffer_fd(int fd, int size);
+IM_API rga_buffer_handle_t importbuffer_virtualaddr(void *va, int size);
+IM_API rga_buffer_handle_t importbuffer_physicaladdr(uint64_t pa, int size);
+#endif
+
+/**
+ * Import external buffers into RGA driver.
+ *
+ * @param fd/va/pa
+ *      Select dma_fd/virtual_address/physical_address by buffer type
+ * @param width
+ *      Describes the pixel width stride of the image buffer
+ * @param height
+ *      Describes the pixel height stride of the image buffer
+ * @param format
+ *      Describes the pixel format of the image buffer
+ *
+ * @return rga_buffer_handle_t
+ */
+#ifdef __cplusplus
+IM_API rga_buffer_handle_t importbuffer_fd(int fd, int width, int height, int format);
+IM_API rga_buffer_handle_t importbuffer_virtualaddr(void *va, int width, int height, int format);
+IM_API rga_buffer_handle_t importbuffer_physicaladdr(uint64_t pa, int width, int height, int format);
+#endif
+
+/**
+ * Import external buffers into RGA driver.
+ *
+ * @param fd/va/pa
+ *      Select dma_fd/virtual_address/physical_address by buffer type
+ * @param param
+ *      Configure buffer parameters
+ *
+ * @return rga_buffer_handle_t
+ */
+IM_EXPORT_API rga_buffer_handle_t importbuffer_fd(int fd, im_handle_param_t *param);
+IM_EXPORT_API rga_buffer_handle_t importbuffer_virtualaddr(void *va, im_handle_param_t *param);
+IM_EXPORT_API rga_buffer_handle_t importbuffer_physicaladdr(uint64_t pa, im_handle_param_t *param);
+
+/**
+ * Import external buffers into RGA driver.
+ *
+ * @param handle
+ *      rga buffer handle
+ *
+ * @return success or else negative error code.
+ */
+IM_EXPORT_API IM_STATUS releasebuffer_handle(rga_buffer_handle_t handle);
+
+/**
+ * Wrap image Parameters.
+ *
+ * @param handle/virtualaddr/physicaladdr/fd
+ *      RGA buffer handle/virtualaddr/physicaladdr/fd.
+ * @param width
+ *      Width of image manipulation area.
+ * @param height
+ *      Height of image manipulation area.
+ * @param wstride
+ *      Width pixel stride, default (width = wstride).
+ * @param hstride
+ *      Height pixel stride, default (height = hstride).
+ * @param format
+ *      Image format.
+ *
+ * @return rga_buffer_t
+ */
+#define wrapbuffer_handle(handle, width, height, format, ...) \
+    ({ \
+        rga_buffer_t im2d_api_buffer; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            im2d_api_buffer = wrapbuffer_handle_t(handle, width, height, width, height, format); \
+        } else if (__argc == 2){ \
+            im2d_api_buffer = wrapbuffer_handle_t(handle, width, height, __args[0], __args[1], format); \
+        } else { \
+            memset(&im2d_api_buffer, 0x0, sizeof(im2d_api_buffer)); \
+            printf("invalid parameter\n"); \
+        } \
+        im2d_api_buffer; \
+    })
+
+#define wrapbuffer_virtualaddr(vir_addr, width, height, format, ...) \
+    ({ \
+        rga_buffer_t im2d_api_buffer; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            im2d_api_buffer = wrapbuffer_virtualaddr_t(vir_addr, width, height, width, height, format); \
+        } else if (__argc == 2){ \
+            im2d_api_buffer = wrapbuffer_virtualaddr_t(vir_addr, width, height, __args[0], __args[1], format); \
+        } else { \
+            memset(&im2d_api_buffer, 0x0, sizeof(im2d_api_buffer)); \
+            printf("invalid parameter\n"); \
+        } \
+        im2d_api_buffer; \
+    })
+
+#define wrapbuffer_physicaladdr(phy_addr, width, height, format, ...) \
+    ({ \
+        rga_buffer_t im2d_api_buffer; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            im2d_api_buffer = wrapbuffer_physicaladdr_t(phy_addr, width, height, width, height, format); \
+        } else if (__argc == 2){ \
+            im2d_api_buffer = wrapbuffer_physicaladdr_t(phy_addr, width, height, __args[0], __args[1], format); \
+        } else { \
+            memset(&im2d_api_buffer, 0x0, sizeof(im2d_api_buffer)); \
+            printf("invalid parameter\n"); \
+        } \
+        im2d_api_buffer; \
+    })
+
+#define wrapbuffer_fd(fd, width, height, format, ...) \
+    ({ \
+        rga_buffer_t im2d_api_buffer; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            im2d_api_buffer = wrapbuffer_fd_t(fd, width, height, width, height, format); \
+        } else if (__argc == 2){ \
+            im2d_api_buffer = wrapbuffer_fd_t(fd, width, height, __args[0], __args[1], format); \
+        } else { \
+            memset(&im2d_api_buffer, 0x0, sizeof(im2d_api_buffer)); \
+            printf("invalid parameter\n"); \
+        } \
+        im2d_api_buffer; \
+    })
+/* Symbols for define *_t functions */
+IM_C_API rga_buffer_t wrapbuffer_handle_t(rga_buffer_handle_t handle, int width, int height, int wstride, int hstride, int format);
+IM_C_API rga_buffer_t wrapbuffer_virtualaddr_t(void* vir_addr, int width, int height, int wstride, int hstride, int format);
+IM_C_API rga_buffer_t wrapbuffer_physicaladdr_t(void* phy_addr, int width, int height, int wstride, int hstride, int format);
+IM_C_API rga_buffer_t wrapbuffer_fd_t(int fd, int width, int height, int wstride, int hstride, int format);
+
+#ifdef __cplusplus
+#undef wrapbuffer_handle
+IM_API rga_buffer_t wrapbuffer_handle(rga_buffer_handle_t  handle,
+                                      int width, int height, int format);
+IM_API rga_buffer_t wrapbuffer_handle(rga_buffer_handle_t  handle,
+                                      int width, int height, int format,
+                                      int wstride, int hstride);
+#endif
+
+#endif /* #ifndef _im2d_buffer_h_ */

obj_dec/src/3rdparty/rga/RK3588/include/im2d_common.h

@@ -0,0 +1,151 @@
+/*
+ * Copyright (C) 2022 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  Cerf Yu <cerf.yu@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef _im2d_common_h_
+#define _im2d_common_h_
+
+#include "im2d_type.h"
+
+/**
+ * Query RGA basic information, supported resolution, supported format, etc.
+ *
+ * @param name
+ *      RGA_VENDOR
+ *      RGA_VERSION
+ *      RGA_MAX_INPUT
+ *      RGA_MAX_OUTPUT
+ *      RGA_INPUT_FORMAT
+ *      RGA_OUTPUT_FORMAT
+ *      RGA_EXPECTED
+ *      RGA_ALL
+ *
+ * @returns a string describing properties of RGA.
+ */
+IM_EXPORT_API const char* querystring(int name);
+
+/**
+ * String to output the error message
+ *
+ * @param status
+ *      process result value.
+ *
+ * @returns error message.
+ */
+#define imStrError(...) \
+    ({ \
+        const char* im2d_api_err; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            im2d_api_err = imStrError_t(IM_STATUS_INVALID_PARAM); \
+        } else if (__argc == 1){ \
+            im2d_api_err = imStrError_t((IM_STATUS)__args[0]); \
+        } else { \
+            im2d_api_err = ("Fatal error, imStrError() too many parameters\n"); \
+            printf("Fatal error, imStrError() too many parameters\n"); \
+        } \
+        im2d_api_err; \
+    })
+IM_C_API const char* imStrError_t(IM_STATUS status);
+
+/**
+ * check im2d api header file
+ *
+ * @param header_version
+ *      Default is RGA_CURRENT_API_HEADER_VERSION, no need to change if there are no special cases.
+ *
+ * @returns no error or else negative error code.
+ */
+#ifdef __cplusplus
+IM_API IM_STATUS imcheckHeader(im_api_version_t header_version = RGA_CURRENT_API_HEADER_VERSION);
+#endif
+
+/**
+ * check RGA basic information, supported resolution, supported format, etc.
+ *
+ * @param src
+ * @param dst
+ * @param pat
+ * @param src_rect
+ * @param dst_rect
+ * @param pat_rect
+ * @param mode_usage
+ *
+ * @returns no error or else negative error code.
+ */
+#define imcheck(src, dst, src_rect, dst_rect, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_NOERROR; \
+        rga_buffer_t __pat; \
+        im_rect __pat_rect; \
+        memset(&__pat, 0, sizeof(rga_buffer_t)); \
+        memset(&__pat_rect, 0, sizeof(im_rect)); \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imcheck_t(src, dst, __pat, src_rect, dst_rect, __pat_rect, 0); \
+        } else if (__argc == 1){ \
+            __ret = imcheck_t(src, dst, __pat, src_rect, dst_rect, __pat_rect, __args[0]); \
+        } else { \
+            __ret = IM_STATUS_FAILED; \
+            printf("check failed\n"); \
+        } \
+        __ret; \
+    })
+#define imcheck_composite(src, dst, pat, src_rect, dst_rect, pat_rect, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_NOERROR; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imcheck_t(src, dst, pat, src_rect, dst_rect, pat_rect, 0); \
+        } else if (__argc == 1){ \
+            __ret = imcheck_t(src, dst, pat, src_rect, dst_rect, pat_rect, __args[0]); \
+        } else { \
+            __ret = IM_STATUS_FAILED; \
+            printf("check failed\n"); \
+        } \
+        __ret; \
+    })
+IM_C_API IM_STATUS imcheck_t(const rga_buffer_t src, const rga_buffer_t dst, const rga_buffer_t pat,
+                             const im_rect src_rect, const im_rect dst_rect, const im_rect pat_rect, const int mode_usage);
+/* Compatible with the legacy symbol */
+IM_C_API void rga_check_perpare(rga_buffer_t *src, rga_buffer_t *dst, rga_buffer_t *pat,
+                                im_rect *src_rect, im_rect *dst_rect, im_rect *pat_rect, int mode_usage);
+
+/**
+ * block until all execution is complete
+ *
+ * @param release_fence_fd
+ *      RGA job release fence fd
+ *
+ * @returns success or else negative error code.
+ */
+IM_EXPORT_API IM_STATUS imsync(int release_fence_fd);
+
+/**
+ * config
+ *
+ * @param name
+ *      enum IM_CONFIG_NAME
+ * @param value
+ *
+ * @returns success or else negative error code.
+ */
+IM_EXPORT_API IM_STATUS imconfig(IM_CONFIG_NAME name, uint64_t value);
+
+#endif /* #ifndef _im2d_common_h_ */

obj_dec/src/3rdparty/rga/RK3588/include/im2d_expand.h

@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) 2022 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  Cerf Yu <cerf.yu@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef _im2d_expand_h_
+#define _im2d_expand_h_
+
+#ifdef __cplusplus
+
+#include "im2d_type.h"
+
+// #if ANDROID
+
+// #include <ui/GraphicBuffer.h>
+
+// using namespace android;
+
+// IM_API rga_buffer_handle_t importbuffer_GraphicBuffer_handle(buffer_handle_t hnd);
+// IM_API rga_buffer_handle_t importbuffer_GraphicBuffer(sp<GraphicBuffer> buf);
+
+// IM_API rga_buffer_t wrapbuffer_handle(buffer_handle_t hnd);
+// IM_API rga_buffer_t wrapbuffer_GraphicBuffer(sp<GraphicBuffer> buf);
+
+// #if USE_AHARDWAREBUFFER
+// #include <android/hardware_buffer.h>
+// IM_API rga_buffer_handle_t importbuffer_AHardwareBuffer(AHardwareBuffer *buf);
+// IM_API rga_buffer_t wrapbuffer_AHardwareBuffer(AHardwareBuffer *buf);
+
+// #endif /* #if USE_AHARDWAREBUFFER */
+// #endif /* #if ANDROID */
+
+#endif /* #ifdef __cplusplus */
+
+#endif

obj_dec/src/3rdparty/rga/RK3588/include/im2d_mpi.h

@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2022 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  Cerf Yu <cerf.yu@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef _im2d_mpi_hpp_
+#define _im2d_mpi_hpp_
+
+#include "im2d_type.h"
+
+/**
+ * Create and config an rga ctx for rockit-ko
+ *
+ * @param flags
+ *      Some configuration flags for this job
+ *
+ * @returns job id.
+ */
+IM_EXPORT_API im_ctx_id_t imbegin(uint32_t flags);
+
+/**
+ * Cancel and delete an rga ctx for rockit-ko
+ *
+ * @param flags
+ *      Some configuration flags for this job
+ *
+ * @returns success or else negative error code.
+ */
+IM_EXPORT_API IM_STATUS imcancel(im_ctx_id_t id);
+
+/**
+ * process for rockit-ko
+ *
+ * @param src
+ *      The input source image and is also the foreground image in blend.
+ * @param dst
+ *      The output destination image and is also the foreground image in blend.
+ * @param pat
+ *      The foreground image, or a LUT table.
+ * @param srect
+ *      The rectangle on the src channel image that needs to be processed.
+ * @param drect
+ *      The rectangle on the dst channel image that needs to be processed.
+ * @param prect
+ *      The rectangle on the pat channel image that needs to be processed.
+ * @param acquire_fence_fd
+ * @param release_fence_fd
+ * @param opt
+ *      The image processing options configuration.
+ * @param usage
+ *      The image processing usage.
+ * @param ctx_id
+ *      ctx id
+ *
+ * @returns success or else negative error code.
+ */
+#ifdef __cplusplus
+IM_API IM_STATUS improcess(rga_buffer_t src, rga_buffer_t dst, rga_buffer_t pat,
+                           im_rect srect, im_rect drect, im_rect prect,
+                           int acquire_fence_fd, int *release_fence_fd,
+                           im_opt_t *opt, int usage, im_ctx_id_t ctx_id);
+#endif
+IM_EXPORT_API IM_STATUS improcess_ctx(rga_buffer_t src, rga_buffer_t dst, rga_buffer_t pat,
+                                      im_rect srect, im_rect drect, im_rect prect,
+                                      int acquire_fence_fd, int *release_fence_fd,
+                                      im_opt_t *opt, int usage, im_ctx_id_t ctx_id);
+
+#endif /* #ifndef _im2d_mpi_hpp_ */

obj_dec/src/3rdparty/rga/RK3588/include/im2d_single.h

@@ -0,0 +1,940 @@
+/*
+ * Copyright (C) 2022 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  Cerf Yu <cerf.yu@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef _im2d_single_h_
+#define _im2d_single_h_
+
+#include "im2d_type.h"
+
+#ifdef __cplusplus
+
+/**
+ * copy
+ *
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ * @param release_fence_fd
+ *      When 'sync == 0', the fence_fd used to identify the current job state
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imcopy(const rga_buffer_t src, rga_buffer_t dst, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * Resize
+ *
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param fx
+ *      X-direction resize factor.
+ * @param fy
+ *      X-direction resize factor.
+ * @param interpolation
+ *      Interpolation formula(Only RGA1 support).
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ * @param release_fence_fd
+ *      When 'sync == 0', the fence_fd used to identify the current job state
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imresize(const rga_buffer_t src, rga_buffer_t dst, double fx = 0, double fy = 0, int interpolation = 0, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * Crop
+ *
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param rect
+ *      The rectangle on the source image that needs to be cropped.
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ * @param release_fence_fd
+ *      When 'sync == 0', the fence_fd used to identify the current job state
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imcrop(const rga_buffer_t src, rga_buffer_t dst, im_rect rect, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * translate
+ *
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param x
+ *      Output the coordinates of the starting point in the X-direction of the destination image.
+ * @param y
+ *      Output the coordinates of the starting point in the Y-direction of the destination image.
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ * @param release_fence_fd
+ *      When 'sync == 0', the fence_fd used to identify the current job state
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imtranslate(const rga_buffer_t src, rga_buffer_t dst, int x, int y, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * format convert
+ *
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param sfmt
+ *      The source image format.
+ * @param dfmt
+ *      The destination image format.
+ * @param mode
+ *      color space mode:
+ *          IM_YUV_TO_RGB_BT601_LIMIT
+ *          IM_YUV_TO_RGB_BT601_FULL
+ *          IM_YUV_TO_RGB_BT709_LIMIT
+ *          IM_RGB_TO_YUV_BT601_FULL
+ *          IM_RGB_TO_YUV_BT601_LIMIT
+ *          IM_RGB_TO_YUV_BT709_LIMIT
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ * @param release_fence_fd
+ *      When 'sync == 0', the fence_fd used to identify the current job state
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imcvtcolor(rga_buffer_t src, rga_buffer_t dst, int sfmt, int dfmt, int mode = IM_COLOR_SPACE_DEFAULT, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * rotation
+ *
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param rotation
+ *      IM_HAL_TRANSFORM_ROT_90
+ *      IM_HAL_TRANSFORM_ROT_180
+ *      IM_HAL_TRANSFORM_ROT_270
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ * @param release_fence_fd
+ *      When 'sync == 0', the fence_fd used to identify the current job state
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imrotate(const rga_buffer_t src, rga_buffer_t dst, int rotation, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * flip
+ *
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param mode
+ *      IM_HAL_TRANSFORM_FLIP_H
+ *      IM_HAL_TRANSFORM_FLIP_V
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ * @param release_fence_fd
+ *      When 'sync == 0', the fence_fd used to identify the current job state
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imflip(const rga_buffer_t src, rga_buffer_t dst, int mode, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * 2-channel blend (SRC + DST -> DST or SRCA + SRCB -> DST)
+ *
+ * @param fg_image
+ *      The foreground image.
+ * @param bg_image
+ *      The background image, which is also the output destination image.
+ * @param mode
+ *      Port-Duff mode:
+ *          IM_ALPHA_BLEND_SRC
+ *          IM_ALPHA_BLEND_DST
+ *          IM_ALPHA_BLEND_SRC_OVER
+ *          IM_ALPHA_BLEND_DST_OVER
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ * @param release_fence_fd
+ *      When 'sync == 0', the fence_fd used to identify the current job state
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imblend(const rga_buffer_t fd_image, rga_buffer_t bg_image, int mode = IM_ALPHA_BLEND_SRC_OVER, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * 3-channel blend (SRC + DST -> DST or SRCA + SRCB -> DST)
+ *
+ * @param fg_image
+ *      The foreground image.
+ * @param bg_image
+ *      The background image.
+ * @param output_image
+ *      The output destination image.
+ * @param mode
+ *      Port-Duff mode:
+ *          IM_ALPHA_BLEND_SRC
+ *          IM_ALPHA_BLEND_DST
+ *          IM_ALPHA_BLEND_SRC_OVER
+ *          IM_ALPHA_BLEND_DST_OVER
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ * @param release_fence_fd
+ *      When 'sync == 0', the fence_fd used to identify the current job state
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imcomposite(const rga_buffer_t srcA, const rga_buffer_t srcB, rga_buffer_t dst, int mode = IM_ALPHA_BLEND_SRC_OVER, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * color key
+ *
+ * @param fg_image
+ *      The foreground image.
+ * @param bg_image
+ *      The background image, which is also the output destination image.
+ * @param colorkey_range
+ *      The range of color key.
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imcolorkey(const rga_buffer_t src, rga_buffer_t dst, im_colorkey_range range, int mode = IM_ALPHA_COLORKEY_NORMAL, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * OSD
+ *
+ * @param osd
+ *      The osd text block.
+ * @param dst
+ *      The background image.
+ * @param osd_rect
+ *      The rectangle on the source image that needs to be OSD.
+ * @param osd_config
+ *      osd mode configuration.
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imosd(const rga_buffer_t osd,const rga_buffer_t dst,
+                       const im_rect osd_rect, im_osd_t *osd_config,
+                       int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * nn quantize
+ *
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param nninfo
+ *      nn configuration
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imquantize(const rga_buffer_t src, rga_buffer_t dst, im_nn_t nn_info, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * ROP
+ *
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param rop_code
+ *      The ROP opcode.
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imrop(const rga_buffer_t src, rga_buffer_t dst, int rop_code, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * fill/reset/draw
+ *
+ * @param dst
+ *      The output destination image.
+ * @param rect
+ *      The rectangle on the source image that needs to be filled with color.
+ * @param color
+ *      The fill color value.
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imfill(rga_buffer_t dst, im_rect rect, int color, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * fill array
+ *
+ * @param dst
+ *      The output destination image.
+ * @param rect_array
+ *      The rectangle arrays on the source image that needs to be filled with color.
+ * @param array_size
+ *      The size of rectangular area arrays.
+ * @param color
+ *      The fill color value.
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imfillArray(rga_buffer_t dst, im_rect *rect_array, int array_size, uint32_t color, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * fill rectangle
+ *
+ * @param dst
+ *      The output destination image.
+ * @param rect
+ *      The rectangle on the source image that needs to be filled with color.
+ * @param color
+ *      The fill color value.
+ * @param thickness
+ *      Thickness of lines that make up the rectangle. Negative values, like -1,
+ *      mean that the function has to draw a filled rectangle.
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imrectangle(rga_buffer_t dst, im_rect rect,
+                             uint32_t color, int thickness,
+                             int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * fill rectangle array
+ *
+ * @param dst
+ *      The output destination image.
+ * @param rect_array
+ *      The rectangle arrays on the source image that needs to be filled with color.
+ * @param array_size
+ *      The size of rectangular area arrays.
+ * @param color
+ *      The fill color value.
+ * @param thickness
+ *      Thickness of lines that make up the rectangle. Negative values, like -1,
+ *      mean that the function has to draw a filled rectangle.
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imrectangleArray(rga_buffer_t dst, im_rect *rect_array, int array_size,
+                                   uint32_t color, int thickness,
+                                   int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * MOSAIC
+ *
+ * @param image
+ *      The output destination image.
+ * @param rect
+ *      The rectangle on the source image that needs to be mosaicked.
+ * @param mosaic_mode
+ *      mosaic block width configuration:
+ *          IM_MOSAIC_8
+ *          IM_MOSAIC_16
+ *          IM_MOSAIC_32
+ *          IM_MOSAIC_64
+ *          IM_MOSAIC_128
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS immosaic(const rga_buffer_t image, im_rect rect, int mosaic_mode, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * MOSAIC array
+ *
+ * @param image
+ *      The output destination image.
+ * @param rect_array
+ *      The rectangle arrays on the source image that needs to be filled with color.
+ * @param array_size
+ *      The size of rectangular area arrays.
+ * @param mosaic_mode
+ *      mosaic block width configuration:
+ *          IM_MOSAIC_8
+ *          IM_MOSAIC_16
+ *          IM_MOSAIC_32
+ *          IM_MOSAIC_64
+ *          IM_MOSAIC_128
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS immosaicArray(const rga_buffer_t image, im_rect *rect_array, int array_size, int mosaic_mode, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * palette
+ *
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param lut
+ *      The LUT table.
+ * @param sync
+ *      When 'sync == 1', wait for the operation to complete and return, otherwise return directly.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS impalette(rga_buffer_t src, rga_buffer_t dst, rga_buffer_t lut, int sync = 1, int *release_fence_fd = NULL);
+
+/**
+ * process for single task mode
+ *
+ * @param src
+ *      The input source image and is also the foreground image in blend.
+ * @param dst
+ *      The output destination image and is also the foreground image in blend.
+ * @param pat
+ *      The foreground image, or a LUT table.
+ * @param srect
+ *      The rectangle on the src channel image that needs to be processed.
+ * @param drect
+ *      The rectangle on the dst channel image that needs to be processed.
+ * @param prect
+ *      The rectangle on the pat channel image that needs to be processed.
+ * @param opt
+ *      The image processing options configuration.
+ * @param usage
+ *      The image processing usage.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS improcess(rga_buffer_t src, rga_buffer_t dst, rga_buffer_t pat,
+                           im_rect srect, im_rect drect, im_rect prect,
+                           int acquire_fence_fd, int *release_fence_fd,
+                           im_opt_t *opt_ptr, int usage);
+
+/**
+ * make border
+ *
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param top
+ *      the top pixels
+ * @param bottom
+ *      the bottom pixels
+ * @param left
+ *      the left pixels
+ * @param right
+ *      the right pixels
+ * @param border_type
+ *      Border type.
+ * @param value
+ *      The pixel value at which the border is filled.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS immakeBorder(rga_buffer_t src, rga_buffer_t dst,
+                              int top, int bottom, int left, int right,
+                              int border_type, int value = 0,
+                              int sync = 1, int acquir_fence_fd = -1, int *release_fence_fd = NULL);
+
+#endif /* #ifdef __cplusplus */
+
+IM_C_API IM_STATUS immosaic(const rga_buffer_t image, im_rect rect, int mosaic_mode, int sync);
+IM_C_API IM_STATUS imosd(const rga_buffer_t osd,const rga_buffer_t dst,
+                         const im_rect osd_rect, im_osd_t *osd_config, int sync);
+IM_C_API IM_STATUS improcess(rga_buffer_t src, rga_buffer_t dst, rga_buffer_t pat,
+                             im_rect srect, im_rect drect, im_rect prect, int usage);
+
+/* Start: Symbols reserved for compatibility with macro functions */
+IM_C_API IM_STATUS imcopy_t(const rga_buffer_t src, rga_buffer_t dst, int sync);
+IM_C_API IM_STATUS imresize_t(const rga_buffer_t src, rga_buffer_t dst, double fx, double fy, int interpolation, int sync);
+IM_C_API IM_STATUS imcrop_t(const rga_buffer_t src, rga_buffer_t dst, im_rect rect, int sync);
+IM_C_API IM_STATUS imtranslate_t(const rga_buffer_t src, rga_buffer_t dst, int x, int y, int sync);
+IM_C_API IM_STATUS imcvtcolor_t(rga_buffer_t src, rga_buffer_t dst, int sfmt, int dfmt, int mode, int sync);
+IM_C_API IM_STATUS imrotate_t(const rga_buffer_t src, rga_buffer_t dst, int rotation, int sync);
+IM_C_API IM_STATUS imflip_t (const rga_buffer_t src, rga_buffer_t dst, int mode, int sync);
+IM_C_API IM_STATUS imblend_t(const rga_buffer_t srcA, const rga_buffer_t srcB, rga_buffer_t dst, int mode, int sync);
+IM_C_API IM_STATUS imcolorkey_t(const rga_buffer_t src, rga_buffer_t dst, im_colorkey_range range, int mode, int sync);
+IM_C_API IM_STATUS imquantize_t(const rga_buffer_t src, rga_buffer_t dst, im_nn_t nn_info, int sync);
+IM_C_API IM_STATUS imrop_t(const rga_buffer_t src, rga_buffer_t dst, int rop_code, int sync);
+IM_C_API IM_STATUS imfill_t(rga_buffer_t dst, im_rect rect, int color, int sync);
+IM_C_API IM_STATUS impalette_t(rga_buffer_t src, rga_buffer_t dst, rga_buffer_t lut, int sync);
+/* End: Symbols reserved for compatibility with macro functions */
+
+#ifndef __cplusplus
+
+#define RGA_GET_MIN(n1, n2) ((n1) < (n2) ? (n1) : (n2))
+
+/**
+ * copy
+ *
+ * @param src
+ * @param dst
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define imcopy(src, dst, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imcopy_t(src, dst, 1); \
+        } else if (__argc == 1){ \
+            __ret = imcopy_t(src, dst, (int)__args[RGA_GET_MIN(__argc, 0)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+/**
+ * Resize
+ *
+ * @param src
+ * @param dst
+ * @param fx
+ * @param fy
+ * @param interpolation
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define imresize(src, dst, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        double __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(double); \
+        if (__argc == 0) { \
+            __ret = imresize_t(src, dst, 0, 0, INTER_LINEAR, 1); \
+        } else if (__argc == 2){ \
+            __ret = imresize_t(src, dst, __args[RGA_GET_MIN(__argc, 0)], __args[RGA_GET_MIN(__argc, 1)], INTER_LINEAR, 1); \
+        } else if (__argc == 3){ \
+            __ret = imresize_t(src, dst, __args[RGA_GET_MIN(__argc, 0)], __args[RGA_GET_MIN(__argc, 1)], (int)__args[RGA_GET_MIN(__argc, 2)], 1); \
+        } else if (__argc == 4){ \
+            __ret = imresize_t(src, dst, __args[RGA_GET_MIN(__argc, 0)], __args[RGA_GET_MIN(__argc, 1)], (int)__args[RGA_GET_MIN(__argc, 2)], (int)__args[RGA_GET_MIN(__argc, 3)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+#define impyramid(src, dst, direction) \
+        imresize_t(src, \
+                   dst, \
+                   direction == IM_UP_SCALE ? 0.5 : 2, \
+                   direction == IM_UP_SCALE ? 0.5 : 2, \
+                   INTER_LINEAR, 1)
+
+/**
+ * format convert
+ *
+ * @param src
+ * @param dst
+ * @param sfmt
+ * @param dfmt
+ * @param mode
+ *      color space mode: IM_COLOR_SPACE_MODE
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define imcvtcolor(src, dst, sfmt, dfmt, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imcvtcolor_t(src, dst, sfmt, dfmt, IM_COLOR_SPACE_DEFAULT, 1); \
+        } else if (__argc == 1){ \
+            __ret = imcvtcolor_t(src, dst, sfmt, dfmt, (int)__args[RGA_GET_MIN(__argc, 0)], 1); \
+        } else if (__argc == 2){ \
+            __ret = imcvtcolor_t(src, dst, sfmt, dfmt, (int)__args[RGA_GET_MIN(__argc, 0)], (int)__args[RGA_GET_MIN(__argc, 1)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+/**
+ * Crop
+ *
+ * @param src
+ * @param dst
+ * @param rect
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define imcrop(src, dst, rect, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imcrop_t(src, dst, rect, 1); \
+        } else if (__argc == 1){ \
+            __ret = imcrop_t(src, dst, rect, (int)__args[RGA_GET_MIN(__argc, 0)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+/**
+ * translate
+ *
+ * @param src
+ * @param dst
+ * @param x
+ * @param y
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define imtranslate(src, dst, x, y, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imtranslate_t(src, dst, x, y, 1); \
+        } else if (__argc == 1){ \
+            __ret = imtranslate_t(src, dst, x, y, (int)__args[RGA_GET_MIN(__argc, 0)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+/**
+ * rotation
+ *
+ * @param src
+ * @param dst
+ * @param rotation
+ *      IM_HAL_TRANSFORM_ROT_90
+ *      IM_HAL_TRANSFORM_ROT_180
+ *      IM_HAL_TRANSFORM_ROT_270
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define imrotate(src, dst, rotation, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imrotate_t(src, dst, rotation, 1); \
+        } else if (__argc == 1){ \
+            __ret = imrotate_t(src, dst, rotation, (int)__args[RGA_GET_MIN(__argc, 0)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+
+/**
+ * flip
+ *
+ * @param src
+ * @param dst
+ * @param mode
+ *      IM_HAL_TRANSFORM_FLIP_H
+ *      IM_HAL_TRANSFORM_FLIP_V
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define imflip(src, dst, mode, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imflip_t(src, dst, mode, 1); \
+        } else if (__argc == 1){ \
+            __ret = imflip_t(src, dst, mode, (int)__args[RGA_GET_MIN(__argc, 0)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+/**
+ * blend (SRC + DST -> DST or SRCA + SRCB -> DST)
+ *
+ * @param srcA
+ * @param srcB can be NULL.
+ * @param dst
+ * @param mode
+ *      IM_ALPHA_BLEND_MODE
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define imblend(srcA, dst, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        rga_buffer_t srcB; \
+        memset(&srcB, 0x00, sizeof(rga_buffer_t)); \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imblend_t(srcA, srcB, dst, IM_ALPHA_BLEND_SRC_OVER, 1); \
+        } else if (__argc == 1){ \
+            __ret = imblend_t(srcA, srcB, dst, (int)__args[RGA_GET_MIN(__argc, 0)], 1); \
+        } else if (__argc == 2){ \
+            __ret = imblend_t(srcA, srcB, dst, (int)__args[RGA_GET_MIN(__argc, 0)], (int)__args[RGA_GET_MIN(__argc, 1)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+#define imcomposite(srcA, srcB, dst, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imblend_t(srcA, srcB, dst, IM_ALPHA_BLEND_SRC_OVER, 1); \
+        } else if (__argc == 1){ \
+            __ret = imblend_t(srcA, srcB, dst, (int)__args[RGA_GET_MIN(__argc, 0)], 1); \
+        } else if (__argc == 2){ \
+            __ret = imblend_t(srcA, srcB, dst, (int)__args[RGA_GET_MIN(__argc, 0)], (int)__args[RGA_GET_MIN(__argc, 1)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+/**
+ * color key
+ *
+ * @param src
+ * @param dst
+ * @param colorkey_range
+ *      max color
+ *      min color
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define imcolorkey(src, dst, range, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imcolorkey_t(src, dst, range, IM_ALPHA_COLORKEY_NORMAL, 1); \
+        } else if (__argc == 1){ \
+            __ret = imcolorkey_t(src, dst, range, (int)__args[RGA_GET_MIN(__argc, 0)], 1); \
+        } else if (__argc == 2){ \
+            __ret = imcolorkey_t(src, dst, range, (int)__args[RGA_GET_MIN(__argc, 0)], (int)__args[RGA_GET_MIN(__argc, 1)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+/**
+ * nn quantize
+ *
+ * @param src
+ * @param dst
+ * @param nninfo
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define imquantize(src, dst, nn_info, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imquantize_t(src, dst, nn_info, 1); \
+        } else if (__argc == 1){ \
+            __ret = imquantize_t(src, dst, nn_info, (int)__args[RGA_GET_MIN(__argc, 0)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+
+/**
+ * ROP
+ *
+ * @param src
+ * @param dst
+ * @param rop_code
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define imrop(src, dst, rop_code, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imrop_t(src, dst, rop_code, 1); \
+        } else if (__argc == 1){ \
+            __ret = imrop_t(src, dst, rop_code, (int)__args[RGA_GET_MIN(__argc, 0)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+/**
+ * fill/reset/draw
+ *
+ * @param src
+ * @param dst
+ * @param rect
+ * @param color
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define imfill(buf, rect, color, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imfill_t(buf, rect, color, 1); \
+        } else if (__argc == 1){ \
+            __ret = imfill_t(buf, rect, color, (int)__args[RGA_GET_MIN(__argc, 0)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+#define imreset(buf, rect, color, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imfill_t(buf, rect, color, 1); \
+        } else if (__argc == 1){ \
+            __ret = imfill_t(buf, rect, color, (int)__args[RGA_GET_MIN(__argc, 0)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+#define imdraw(buf, rect, color, ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = imfill_t(buf, rect, color, 1); \
+        } else if (__argc == 1){ \
+            __ret = imfill_t(buf, rect, color, (int)__args[RGA_GET_MIN(__argc, 0)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+
+/**
+ * palette
+ *
+ * @param src
+ * @param dst
+ * @param lut
+ * @param sync
+ *      wait until operation complete
+ *
+ * @returns success or else negative error code.
+ */
+#define impalette(src, dst, lut,  ...) \
+    ({ \
+        IM_STATUS __ret = IM_STATUS_SUCCESS; \
+        int __args[] = {__VA_ARGS__}; \
+        int __argc = sizeof(__args)/sizeof(int); \
+        if (__argc == 0) { \
+            __ret = impalette_t(src, dst, lut, 1); \
+        } else if (__argc == 1){ \
+            __ret = impalette_t(src, dst, lut, (int)__args[RGA_GET_MIN(__argc, 0)]); \
+        } else { \
+            __ret = IM_STATUS_INVALID_PARAM; \
+            printf("invalid parameter\n"); \
+        } \
+        __ret; \
+    })
+/* End define IM2D macro API */
+#endif
+
+#endif /* #ifndef _im2d_single_h_ */

obj_dec/src/3rdparty/rga/RK3588/include/im2d_task.h

@@ -0,0 +1,497 @@
+/*
+ * Copyright (C) 2022 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  Cerf Yu <cerf.yu@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef _im2d_task_h_
+#define _im2d_task_h_
+
+#include "im2d_type.h"
+
+#ifdef __cplusplus
+
+/**
+ * Create an rga job
+ *
+ * @param flags
+ *      Some configuration flags for this job
+ *
+ * @returns job handle.
+ */
+IM_API im_job_handle_t imbeginJob(uint64_t flags = 0);
+
+/**
+ * Submit and run an rga job
+ *
+ * @param job_handle
+ *      This is the job handle that will be submitted.
+ * @param sync_mode
+ *      run mode:
+ *          IM_SYNC
+ *          IM_ASYNC
+ * @param acquire_fence_fd
+ * @param release_fence_fd
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imendJob(im_job_handle_t job_handle,
+                          int sync_mode = IM_SYNC,
+                          int acquire_fence_fd = 0, int *release_fence_fd = NULL);
+
+/**
+ * Cancel and delete an rga job
+ *
+ * @param job_handle
+ *      This is the job handle that will be cancelled.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imcancelJob(im_job_handle_t job_handle);
+
+/**
+ * Add copy task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imcopyTask(im_job_handle_t job_handle, const rga_buffer_t src, rga_buffer_t dst);
+
+/**
+ * Add resize task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param fx
+ *      X-direction resize factor.
+ * @param fy
+ *      X-direction resize factor.
+ * @param interpolation
+ *      Interpolation formula(Only RGA1 support).
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imresizeTask(im_job_handle_t job_handle,
+                              const rga_buffer_t src, rga_buffer_t dst,
+                              double fx = 0, double fy = 0,
+                              int interpolation = 0);
+
+/**
+ * Add crop task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param rect
+ *      The rectangle on the source image that needs to be cropped.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imcropTask(im_job_handle_t job_handle,
+                            const rga_buffer_t src, rga_buffer_t dst, im_rect rect);
+
+/**
+ * Add translate task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param x
+ *      Output the coordinates of the starting point in the X-direction of the destination image.
+ * @param y
+ *      Output the coordinates of the starting point in the Y-direction of the destination image.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imtranslateTask(im_job_handle_t job_handle,
+                                 const rga_buffer_t src, rga_buffer_t dst, int x, int y);
+
+/**
+ * Add format convert task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param sfmt
+ *      The source image format.
+ * @param dfmt
+ *      The destination image format.
+ * @param mode
+ *      color space mode:
+ *          IM_YUV_TO_RGB_BT601_LIMIT
+ *          IM_YUV_TO_RGB_BT601_FULL
+ *          IM_YUV_TO_RGB_BT709_LIMIT
+ *          IM_RGB_TO_YUV_BT601_FULL
+ *          IM_RGB_TO_YUV_BT601_LIMIT
+ *          IM_RGB_TO_YUV_BT709_LIMIT
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imcvtcolorTask(im_job_handle_t job_handle,
+                                rga_buffer_t src, rga_buffer_t dst,
+                                int sfmt, int dfmt, int mode = IM_COLOR_SPACE_DEFAULT);
+
+/**
+ * Add rotation task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param rotation
+ *      IM_HAL_TRANSFORM_ROT_90
+ *      IM_HAL_TRANSFORM_ROT_180
+ *      IM_HAL_TRANSFORM_ROT_270
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imrotateTask(im_job_handle_t job_handle,
+                              const rga_buffer_t src, rga_buffer_t dst, int rotation);
+
+/**
+ * Add flip task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param mode
+ *      IM_HAL_TRANSFORM_FLIP_H
+ *      IM_HAL_TRANSFORM_FLIP_V
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imflipTask(im_job_handle_t job_handle,
+                            const rga_buffer_t src, rga_buffer_t dst, int mode);
+
+/**
+ * Add blend(SRC + DST -> DST) task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param fg_image
+ *      The foreground image.
+ * @param bg_image
+ *      The background image, which is also the output destination image.
+ * @param mode
+ *      Port-Duff mode:
+ *          IM_ALPHA_BLEND_SRC
+ *          IM_ALPHA_BLEND_DST
+ *          IM_ALPHA_BLEND_SRC_OVER
+ *          IM_ALPHA_BLEND_DST_OVER
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imblendTask(im_job_handle_t job_handle,
+                             const rga_buffer_t fg_image, rga_buffer_t bg_image,
+                             int mode = IM_ALPHA_BLEND_SRC_OVER);
+
+/**
+ * Add composite(SRCA + SRCB -> DST) task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param fg_image
+ *      The foreground image.
+ * @param bg_image
+ *      The background image.
+ * @param output_image
+ *      The output destination image.
+ * @param mode
+ *      Port-Duff mode:
+ *          IM_ALPHA_BLEND_SRC
+ *          IM_ALPHA_BLEND_DST
+ *          IM_ALPHA_BLEND_SRC_OVER
+ *          IM_ALPHA_BLEND_DST_OVER
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imcompositeTask(im_job_handle_t job_handle,
+                                 const rga_buffer_t fg_image, const rga_buffer_t bg_image,
+                                 rga_buffer_t output_image,
+                                 int mode = IM_ALPHA_BLEND_SRC_OVER);
+
+/**
+ * Add color key task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param fg_image
+ *      The foreground image.
+ * @param bg_image
+ *      The background image, which is also the output destination image.
+ * @param colorkey_range
+ *      The range of color key.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imcolorkeyTask(im_job_handle_t job_handle,
+                                const rga_buffer_t fg_image, rga_buffer_t bg_image,
+                                im_colorkey_range range, int mode = IM_ALPHA_COLORKEY_NORMAL);
+
+/**
+ * Add OSD task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param osd
+ *      The osd text block.
+ * @param dst
+ *      The background image.
+ * @param osd_rect
+ *      The rectangle on the source image that needs to be OSD.
+ * @param osd_config
+ *      osd mode configuration.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imosdTask(im_job_handle_t job_handle,
+                           const rga_buffer_t osd,const rga_buffer_t bg_image,
+                           const im_rect osd_rect, im_osd_t *osd_config);
+
+/**
+ * Add nn quantize task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param nninfo
+ *      nn configuration
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imquantizeTask(im_job_handle_t job_handle,
+                                const rga_buffer_t src, rga_buffer_t dst, im_nn_t nn_info);
+
+/**
+ * Add ROP task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param rop_code
+ *      The ROP opcode.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imropTask(im_job_handle_t job_handle,
+                           const rga_buffer_t src, rga_buffer_t dst, int rop_code);
+
+/**
+ * Add color fill task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param dst
+ *      The output destination image.
+ * @param rect
+ *      The rectangle on the source image that needs to be filled with color.
+ * @param color
+ *      The fill color value.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imfillTask(im_job_handle_t job_handle, rga_buffer_t dst, im_rect rect, uint32_t color);
+
+/**
+ * Add color fill task array
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param dst
+ *      The output destination image.
+ * @param rect_array
+ *      The rectangle arrays on the source image that needs to be filled with color.
+ * @param array_size
+ *      The size of rectangular area arrays.
+ * @param color
+ *      The fill color value.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imfillTaskArray(im_job_handle_t job_handle,
+                                 rga_buffer_t dst,
+                                 im_rect *rect_array, int array_size, uint32_t color);
+
+/**
+ * Add fill rectangle task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param dst
+ *      The output destination image.
+ * @param rect
+ *      The rectangle on the source image that needs to be filled with color.
+ * @param color
+ *      The fill color value.
+ * @param thickness
+ *      Thickness of lines that make up the rectangle. Negative values, like -1,
+ *      mean that the function has to draw a filled rectangle.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imrectangleTask(im_job_handle_t job_handle,
+                                 rga_buffer_t dst,
+                                 im_rect rect,
+                                 uint32_t color, int thickness);
+
+/**
+ * Add fill rectangle task array
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param dst
+ *      The output destination image.
+ * @param rect_array
+ *      The rectangle arrays on the source image that needs to be filled with color.
+ * @param array_size
+ *      The size of rectangular area arrays.
+ * @param color
+ *      The fill color value.
+ * @param thickness
+ *      Thickness of lines that make up the rectangle. Negative values, like -1,
+ *      mean that the function has to draw a filled rectangle.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS imrectangleTaskArray(im_job_handle_t job_handle,
+                                      rga_buffer_t dst,
+                                      im_rect *rect_array, int array_size,
+                                      uint32_t color, int thickness);
+
+/**
+ * Add mosaic task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param image
+ *      The output destination image.
+ * @param rect
+ *      The rectangle on the source image that needs to be mosaicked.
+ * @param mosaic_mode
+ *      mosaic block width configuration:
+ *          IM_MOSAIC_8
+ *          IM_MOSAIC_16
+ *          IM_MOSAIC_32
+ *          IM_MOSAIC_64
+ *          IM_MOSAIC_128
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS immosaicTask(im_job_handle_t job_handle,
+                              const rga_buffer_t image, im_rect rect, int mosaic_mode);
+
+/**
+ * Add mosaic task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param image
+ *      The output destination image.
+ * @param rect_array
+ *      The rectangle arrays on the source image that needs to be filled with color.
+ * @param array_size
+ *      The size of rectangular area arrays.
+ * @param mosaic_mode
+ *      mosaic block width configuration:
+ *          IM_MOSAIC_8
+ *          IM_MOSAIC_16
+ *          IM_MOSAIC_32
+ *          IM_MOSAIC_64
+ *          IM_MOSAIC_128
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS immosaicTaskArray(im_job_handle_t job_handle,
+                                   const rga_buffer_t image,
+                                   im_rect *rect_array, int array_size, int mosaic_mode);
+
+/**
+ * Add palette task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param src
+ *      The input source image.
+ * @param dst
+ *      The output destination image.
+ * @param lut
+ *      The LUT table.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS impaletteTask(im_job_handle_t job_handle,
+                               rga_buffer_t src, rga_buffer_t dst, rga_buffer_t lut);
+
+/**
+ * Add process task
+ *
+ * @param job_handle
+ *      Insert the task into the job handle.
+ * @param src
+ *      The input source image and is also the foreground image in blend.
+ * @param dst
+ *      The output destination image and is also the foreground image in blend.
+ * @param pat
+ *      The foreground image, or a LUT table.
+ * @param srect
+ *      The rectangle on the src channel image that needs to be processed.
+ * @param drect
+ *      The rectangle on the dst channel image that needs to be processed.
+ * @param prect
+ *      The rectangle on the pat channel image that needs to be processed.
+ * @param opt
+ *      The image processing options configuration.
+ * @param usage
+ *      The image processing usage.
+ *
+ * @returns success or else negative error code.
+ */
+IM_API IM_STATUS improcessTask(im_job_handle_t job_handle,
+                               rga_buffer_t src, rga_buffer_t dst, rga_buffer_t pat,
+                               im_rect srect, im_rect drect, im_rect prect,
+                               im_opt_t *opt_ptr, int usage);
+
+#endif /* #ifdef __cplusplus */
+
+#endif /* #ifndef _im2d_task_h_ */

obj_dec/src/3rdparty/rga/RK3588/include/im2d_type.h

@@ -0,0 +1,436 @@
+/*
+ * Copyright (C) 2022 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  Cerf Yu <cerf.yu@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef _RGA_IM2D_TYPE_H_
+#define _RGA_IM2D_TYPE_H_
+
+#include <stdint.h>
+
+#include "rga.h"
+
+#define IM_API /* define API export as needed */
+
+#ifdef __cplusplus
+#define IM_C_API extern "C"
+#define IM_EXPORT_API extern "C"
+#else
+#define IM_C_API
+#define IM_EXPORT_API
+#endif
+
+#ifdef __cplusplus
+#define DEFAULT_INITIALIZER(x) = x
+#else
+#define DEFAULT_INITIALIZER(x)
+#endif
+
+typedef uint32_t im_api_version_t;
+typedef uint32_t im_job_handle_t;
+typedef uint32_t im_ctx_id_t;
+typedef uint32_t rga_buffer_handle_t;
+
+typedef enum {
+    /* Rotation */
+    IM_HAL_TRANSFORM_ROT_90     = 1 << 0,
+    IM_HAL_TRANSFORM_ROT_180    = 1 << 1,
+    IM_HAL_TRANSFORM_ROT_270    = 1 << 2,
+    IM_HAL_TRANSFORM_FLIP_H     = 1 << 3,
+    IM_HAL_TRANSFORM_FLIP_V     = 1 << 4,
+    IM_HAL_TRANSFORM_FLIP_H_V   = 1 << 5,
+    IM_HAL_TRANSFORM_MASK       = 0x3f,
+
+    /*
+     * Blend
+     * Additional blend usage, can be used with both source and target configs.
+     * If none of the below is set, the default "SRC over DST" is applied.
+     */
+    IM_ALPHA_BLEND_SRC_OVER     = 1 << 6,     /* Default, Porter-Duff "SRC over DST" */
+    IM_ALPHA_BLEND_SRC          = 1 << 7,     /* Porter-Duff "SRC" */
+    IM_ALPHA_BLEND_DST          = 1 << 8,     /* Porter-Duff "DST" */
+    IM_ALPHA_BLEND_SRC_IN       = 1 << 9,     /* Porter-Duff "SRC in DST" */
+    IM_ALPHA_BLEND_DST_IN       = 1 << 10,    /* Porter-Duff "DST in SRC" */
+    IM_ALPHA_BLEND_SRC_OUT      = 1 << 11,    /* Porter-Duff "SRC out DST" */
+    IM_ALPHA_BLEND_DST_OUT      = 1 << 12,    /* Porter-Duff "DST out SRC" */
+    IM_ALPHA_BLEND_DST_OVER     = 1 << 13,    /* Porter-Duff "DST over SRC" */
+    IM_ALPHA_BLEND_SRC_ATOP     = 1 << 14,    /* Porter-Duff "SRC ATOP" */
+    IM_ALPHA_BLEND_DST_ATOP     = 1 << 15,    /* Porter-Duff "DST ATOP" */
+    IM_ALPHA_BLEND_XOR          = 1 << 16,    /* Xor */
+    IM_ALPHA_BLEND_MASK         = 0x1ffc0,
+
+    IM_ALPHA_COLORKEY_NORMAL    = 1 << 17,
+    IM_ALPHA_COLORKEY_INVERTED  = 1 << 18,
+    IM_ALPHA_COLORKEY_MASK      = 0x60000,
+
+    IM_SYNC                     = 1 << 19,
+    IM_CROP                     = 1 << 20,    /* Unused */
+    IM_COLOR_FILL               = 1 << 21,
+    IM_COLOR_PALETTE            = 1 << 22,
+    IM_NN_QUANTIZE              = 1 << 23,
+    IM_ROP                      = 1 << 24,
+    IM_ALPHA_BLEND_PRE_MUL      = 1 << 25,
+    IM_ASYNC                    = 1 << 26,
+    IM_MOSAIC                   = 1 << 27,
+    IM_OSD                      = 1 << 28,
+    IM_PRE_INTR                 = 1 << 29,
+} IM_USAGE;
+
+typedef enum {
+    IM_RASTER_MODE              = 1 << 0,
+    IM_FBC_MODE                 = 1 << 1,
+    IM_TILE_MODE                = 1 << 2,
+} IM_RD_MODE;
+
+typedef enum {
+    IM_SCHEDULER_RGA3_CORE0     = 1 << 0,
+    IM_SCHEDULER_RGA3_CORE1     = 1 << 1,
+    IM_SCHEDULER_RGA2_CORE0     = 1 << 2,
+    IM_SCHEDULER_RGA3_DEFAULT   = IM_SCHEDULER_RGA3_CORE0,
+    IM_SCHEDULER_RGA2_DEFAULT   = IM_SCHEDULER_RGA2_CORE0,
+    IM_SCHEDULER_MASK           = 0x7,
+    IM_SCHEDULER_DEFAULT        = 0,
+} IM_SCHEDULER_CORE;
+
+typedef enum {
+    IM_ROP_AND                  = 0x88,
+    IM_ROP_OR                   = 0xee,
+    IM_ROP_NOT_DST              = 0x55,
+    IM_ROP_NOT_SRC              = 0x33,
+    IM_ROP_XOR                  = 0xf6,
+    IM_ROP_NOT_XOR              = 0xf9,
+} IM_ROP_CODE;
+
+typedef enum {
+    IM_MOSAIC_8                 = 0x0,
+    IM_MOSAIC_16                = 0x1,
+    IM_MOSAIC_32                = 0x2,
+    IM_MOSAIC_64                = 0x3,
+    IM_MOSAIC_128               = 0x4,
+} IM_MOSAIC_MODE;
+
+typedef enum {
+    IM_BORDER_CONSTANT = 0,             /* iiiiii|abcdefgh|iiiiiii with some specified value 'i' */
+    IM_BORDER_REFLECT = 2,              /* fedcba|abcdefgh|hgfedcb */
+    IM_BORDER_WRAP = 3,                 /* cdefgh|abcdefgh|abcdefg */
+} IM_BORDER_TYPE;
+
+/* Status codes, returned by any blit function */
+typedef enum {
+    IM_YUV_TO_RGB_BT601_LIMIT   = 1 << 0,
+    IM_YUV_TO_RGB_BT601_FULL    = 2 << 0,
+    IM_YUV_TO_RGB_BT709_LIMIT   = 3 << 0,
+    IM_YUV_TO_RGB_MASK          = 3 << 0,
+    IM_RGB_TO_YUV_BT601_FULL    = 1 << 2,
+    IM_RGB_TO_YUV_BT601_LIMIT   = 2 << 2,
+    IM_RGB_TO_YUV_BT709_LIMIT   = 3 << 2,
+    IM_RGB_TO_YUV_MASK          = 3 << 2,
+    IM_RGB_TO_Y4                = 1 << 4,
+    IM_RGB_TO_Y4_DITHER         = 2 << 4,
+    IM_RGB_TO_Y1_DITHER         = 3 << 4,
+    IM_Y4_MASK                  = 3 << 4,
+    IM_RGB_FULL                 = 1 << 8,
+    IM_RGB_CLIP                 = 2 << 8,
+    IM_YUV_BT601_LIMIT_RANGE    = 3 << 8,
+    IM_YUV_BT601_FULL_RANGE     = 4 << 8,
+    IM_YUV_BT709_LIMIT_RANGE    = 5 << 8,
+    IM_YUV_BT709_FULL_RANGE     = 6 << 8,
+    IM_FULL_CSC_MASK            = 0xf << 8,
+    IM_COLOR_SPACE_DEFAULT      = 0,
+} IM_COLOR_SPACE_MODE;
+
+typedef enum {
+    IM_UP_SCALE,
+    IM_DOWN_SCALE,
+} IM_SCALE;
+
+typedef enum {
+    INTER_NEAREST,
+    INTER_LINEAR,
+    INTER_CUBIC,
+} IM_SCALE_MODE;
+
+typedef enum {
+    IM_CONFIG_SCHEDULER_CORE,
+    IM_CONFIG_PRIORITY,
+    IM_CONFIG_CHECK,
+} IM_CONFIG_NAME;
+
+typedef enum {
+    IM_OSD_MODE_STATISTICS      = 0x1 << 0,
+    IM_OSD_MODE_AUTO_INVERT     = 0x1 << 1,
+} IM_OSD_MODE;
+
+typedef enum {
+    IM_OSD_INVERT_CHANNEL_NONE          = 0x0,
+    IM_OSD_INVERT_CHANNEL_Y_G           = 0x1 << 0,
+    IM_OSD_INVERT_CHANNEL_C_RB          = 0x1 << 1,
+    IM_OSD_INVERT_CHANNEL_ALPHA         = 0x1 << 2,
+    IM_OSD_INVERT_CHANNEL_COLOR         = IM_OSD_INVERT_CHANNEL_Y_G |
+                                          IM_OSD_INVERT_CHANNEL_C_RB,
+    IM_OSD_INVERT_CHANNEL_BOTH          = IM_OSD_INVERT_CHANNEL_COLOR |
+                                          IM_OSD_INVERT_CHANNEL_ALPHA,
+} IM_OSD_INVERT_CHANNEL;
+
+typedef enum {
+    IM_OSD_FLAGS_INTERNAL = 0,
+    IM_OSD_FLAGS_EXTERNAL,
+} IM_OSD_FLAGS_MODE;
+
+typedef enum {
+    IM_OSD_INVERT_USE_FACTOR,
+    IM_OSD_INVERT_USE_SWAP,
+} IM_OSD_INVERT_MODE;
+
+typedef enum {
+    IM_OSD_BACKGROUND_DEFAULT_BRIGHT = 0,
+    IM_OSD_BACKGROUND_DEFAULT_DARK,
+} IM_OSD_BACKGROUND_DEFAULT;
+
+typedef enum {
+    IM_OSD_BLOCK_MODE_NORMAL = 0,
+    IM_OSD_BLOCK_MODE_DIFFERENT,
+} IM_OSD_BLOCK_WIDTH_MODE;
+
+typedef enum {
+    IM_OSD_MODE_HORIZONTAL,
+    IM_OSD_MODE_VERTICAL,
+} IM_OSD_DIRECTION;
+
+typedef enum {
+    IM_OSD_COLOR_PIXEL,
+    IM_OSD_COLOR_EXTERNAL,
+} IM_OSD_COLOR_MODE;
+
+typedef enum {
+    IM_INTR_READ_INTR           = 1 << 0,
+    IM_INTR_READ_HOLD           = 1 << 1,
+    IM_INTR_WRITE_INTR          = 1 << 2,
+} IM_PRE_INTR_FLAGS;
+
+typedef enum {
+    IM_CONTEXT_NONE             = 0x0,
+    IM_CONTEXT_SRC_FIX_ENABLE   = 0x1 << 0,     // Enable kernel to modify the image parameters of the channel.
+    IM_CONTEXT_SRC_CACHE_INFO   = 0x1 << 1,     // It will replace the parameters in ctx with the modified parameters.
+    IM_CONTEXT_SRC1_FIX_ENABLE  = 0x1 << 2,
+    IM_CONTEXT_SRC1_CACHE_INFO  = 0x1 << 3,
+    IM_CONTEXT_DST_FIX_ENABLE   = 0x1 << 4,
+    IM_CONTEXT_DST_CACHE_INFO   = 0x1 << 5,
+} IM_CONTEXT_FLAGS;
+
+/* Get RGA basic information index */
+typedef enum {
+    RGA_VENDOR = 0,
+    RGA_VERSION,
+    RGA_MAX_INPUT,
+    RGA_MAX_OUTPUT,
+    RGA_BYTE_STRIDE,
+    RGA_SCALE_LIMIT,
+    RGA_INPUT_FORMAT,
+    RGA_OUTPUT_FORMAT,
+    RGA_FEATURE,
+    RGA_EXPECTED,
+    RGA_ALL,
+} IM_INFORMATION;
+
+/* Status codes, returned by any blit function */
+typedef enum {
+    IM_STATUS_NOERROR           =  2,
+    IM_STATUS_SUCCESS           =  1,
+    IM_STATUS_NOT_SUPPORTED     = -1,
+    IM_STATUS_OUT_OF_MEMORY     = -2,
+    IM_STATUS_INVALID_PARAM     = -3,
+    IM_STATUS_ILLEGAL_PARAM     = -4,
+    IM_STATUS_ERROR_VERSION     = -5,
+    IM_STATUS_FAILED            =  0,
+} IM_STATUS;
+
+/* Rectangle definition */
+typedef struct {
+    int x;        /* upper-left x */
+    int y;        /* upper-left y */
+    int width;    /* width */
+    int height;   /* height */
+} im_rect;
+
+typedef struct {
+    int max;                    /* The Maximum value of the color key */
+    int min;                    /* The minimum value of the color key */
+} im_colorkey_range;
+
+
+typedef struct im_nn {
+    int scale_r;                /* scaling factor on R channal */
+    int scale_g;                /* scaling factor on G channal */
+    int scale_b;                /* scaling factor on B channal */
+    int offset_r;               /* offset on R channal */
+    int offset_g;               /* offset on G channal */
+    int offset_b;               /* offset on B channal */
+} im_nn_t;
+
+/* im_info definition */
+typedef struct {
+    void* vir_addr;                     /* virtual address */
+    void* phy_addr;                     /* physical address */
+    int fd;                             /* shared fd */
+
+    int width;                          /* width */
+    int height;                         /* height */
+    int wstride;                        /* wstride */
+    int hstride;                        /* hstride */
+    int format;                         /* format */
+
+    int color_space_mode;               /* color_space_mode */
+    int global_alpha;                   /* global_alpha */
+    int rd_mode;
+
+    /* legarcy */
+    int color;                          /* color, used by color fill */
+    im_colorkey_range colorkey_range;   /* range value of color key */
+    im_nn_t nn;
+    int rop_code;
+
+    rga_buffer_handle_t handle;         /* buffer handle */
+} rga_buffer_t;
+
+typedef struct im_color {
+    union {
+        struct {
+            uint8_t red;
+            uint8_t green;
+            uint8_t blue;
+            uint8_t alpha;
+        };
+        uint32_t value;
+    };
+} im_color_t;
+
+typedef struct im_osd_invert_factor {
+    uint8_t alpha_max;
+    uint8_t alpha_min;
+    uint8_t yg_max;
+    uint8_t yg_min;
+    uint8_t crb_max;
+    uint8_t crb_min;
+} im_osd_invert_factor_t;
+
+typedef struct im_osd_bpp2 {
+    uint8_t  ac_swap;       // ac swap flag
+                            // 0: CA
+                            // 1: AC
+    uint8_t  endian_swap;   // rgba2bpp endian swap
+                            // 0: Big endian
+                            // 1: Little endian
+    im_color_t color0;
+    im_color_t color1;
+} im_osd_bpp2_t;
+
+typedef struct im_osd_block {
+    int width_mode;                 // normal or different
+                                    //   IM_OSD_BLOCK_MODE_NORMAL
+                                    //   IM_OSD_BLOCK_MODE_DIFFERENT
+    union {
+        int width;                  // normal_mode block width
+        int width_index;            // different_mode block width index in RAM
+    };
+
+    int block_count;                // block count
+
+    int background_config;          // background config is bright or dark
+                                    //   IM_OSD_BACKGROUND_DEFAULT_BRIGHT
+                                    //   IM_OSD_BACKGROUND_DEFAULT_DARK
+
+    int direction;                  // osd block direction
+                                    //   IM_OSD_MODE_HORIZONTAL
+                                    //   IM_OSD_MODE_VERTICAL
+
+    int color_mode;                 // using src1 color or config color
+                                    //   IM_OSD_COLOR_PIXEL
+                                    //   IM_OSD_COLOR_EXTERNAL
+    im_color_t normal_color;        // config color: normal
+    im_color_t invert_color;        // config color: invert
+} im_osd_block_t;
+
+typedef struct im_osd_invert {
+    int invert_channel;         // invert channel config:
+                                //   IM_OSD_INVERT_CHANNEL_NONE
+                                //   IM_OSD_INVERT_CHANNEL_Y_G
+                                //   IM_OSD_INVERT_CHANNEL_C_RB
+                                //   IM_OSD_INVERT_CHANNEL_ALPHA
+                                //   IM_OSD_INVERT_CHANNEL_COLOR
+                                //   IM_OSD_INVERT_CHANNEL_BOTH
+    int flags_mode;             // use external or inertnal RAM invert flags
+                                //   IM_OSD_FLAGS_EXTERNAL
+                                //   IM_OSD_FLAGS_INTERNAL
+    int flags_index;            // flags index when using internal RAM invert flags
+
+    uint64_t invert_flags;      // external invert flags
+    uint64_t current_flags;     // current flags
+
+    int invert_mode;            // invert use swap or factor
+                                //   IM_OSD_INVERT_USE_FACTOR
+                                //   IM_OSD_INVERT_USE_SWAP
+    im_osd_invert_factor_t factor;
+
+    int threash;
+} im_osd_invert_t;
+
+typedef struct im_osd {
+    int osd_mode;                       // osd mode: statistics or auto_invert
+                                        //   IM_OSD_MODE_STATISTICS
+                                        //   IM_OSD_MODE_AUTO_INVERT
+    im_osd_block_t block_parm;          // osd block info config
+
+    im_osd_invert_t invert_config;
+
+    im_osd_bpp2_t bpp2_info;
+} im_osd_t;
+
+typedef struct im_intr_config {
+    uint32_t flags;
+
+    int read_threshold;
+    int write_start;
+    int write_step;
+} im_intr_config_t;
+
+typedef struct im_opt {
+    im_api_version_t version DEFAULT_INITIALIZER(RGA_CURRENT_API_HEADER_VERSION);
+
+    int color;                          /* color, used by color fill */
+    im_colorkey_range colorkey_range;   /* range value of color key */
+    im_nn_t nn;
+    int rop_code;
+
+    int priority;
+    int core;
+
+    int mosaic_mode;
+
+    im_osd_t osd_config;
+
+    im_intr_config_t intr_config;
+
+    char reserve[128];
+} im_opt_t;
+
+typedef struct im_handle_param {
+    uint32_t width;
+    uint32_t height;
+    uint32_t format;
+} im_handle_param_t;
+
+#endif /* _RGA_IM2D_TYPE_H_ */

obj_dec/src/3rdparty/rga/RK3588/include/im2d_version.h

@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) 2022 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *  Cerf Yu <cerf.yu@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef _RGA_IM2D_VERSION_H_
+#define _RGA_IM2D_VERSION_H_
+
+#define RGA_VERSION_STR_HELPER(x) #x
+#define RGA_VERSION_STR(x) RGA_VERSION_STR_HELPER(x)
+
+/* RGA im2d api verison */
+#define RGA_API_MAJOR_VERSION       1
+#define RGA_API_MINOR_VERSION       9
+#define RGA_API_REVISION_VERSION    1
+#define RGA_API_BUILD_VERSION       4
+
+#define RGA_API_VERSION \
+    RGA_VERSION_STR(RGA_API_MAJOR_VERSION) "." \
+    RGA_VERSION_STR(RGA_API_MINOR_VERSION) "." \
+    RGA_VERSION_STR(RGA_API_REVISION_VERSION) "_[" \
+    RGA_VERSION_STR(RGA_API_BUILD_VERSION) "]"
+#define RGA_API_FULL_VERSION "rga_api version " RGA_API_VERSION
+
+/* For header file version verification */
+#define RGA_CURRENT_API_VERSION (\
+    (RGA_API_MAJOR_VERSION & 0xff) << 24 | \
+    (RGA_API_MINOR_VERSION & 0xff) << 16 | \
+    (RGA_API_REVISION_VERSION & 0xff) << 8 | \
+    (RGA_API_BUILD_VERSION & 0xff)\
+    )
+#define RGA_CURRENT_API_HEADER_VERSION RGA_CURRENT_API_VERSION
+
+#endif /* _RGA_IM2D_VERSION_H_ */

obj_dec/src/3rdparty/rga/RK3588/include/rga.h

@@ -0,0 +1,139 @@
+/*
+ * Copyright (C) 2016 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *    Zhiqin Wei <wzq@rock-chips.com>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef _RGA_DRIVER_H_
+#define _RGA_DRIVER_H_
+
+
+#ifndef ENABLE
+#define ENABLE 1
+#endif
+
+#ifndef DISABLE
+#define DISABLE 0
+#endif
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/* In order to be compatible with RK_FORMAT_XX and HAL_PIXEL_FORMAT_XX,
+ * RK_FORMAT_XX is shifted to the left by 8 bits to distinguish.  */
+typedef enum _Rga_SURF_FORMAT {
+    RK_FORMAT_RGBA_8888    = 0x0 << 8,
+    RK_FORMAT_RGBX_8888    = 0x1 << 8,
+    RK_FORMAT_RGB_888      = 0x2 << 8,
+    RK_FORMAT_BGRA_8888    = 0x3 << 8,
+    RK_FORMAT_RGB_565      = 0x4 << 8,
+    RK_FORMAT_RGBA_5551    = 0x5 << 8,
+    RK_FORMAT_RGBA_4444    = 0x6 << 8,
+    RK_FORMAT_BGR_888      = 0x7 << 8,
+
+    RK_FORMAT_YCbCr_422_SP = 0x8 << 8,
+    RK_FORMAT_YCbCr_422_P  = 0x9 << 8,
+    RK_FORMAT_YCbCr_420_SP = 0xa << 8,
+    RK_FORMAT_YCbCr_420_P  = 0xb << 8,
+
+    RK_FORMAT_YCrCb_422_SP = 0xc << 8,
+    RK_FORMAT_YCrCb_422_P  = 0xd << 8,
+    RK_FORMAT_YCrCb_420_SP = 0xe << 8,
+    RK_FORMAT_YCrCb_420_P  = 0xf << 8,
+
+    RK_FORMAT_BPP1         = 0x10 << 8,
+    RK_FORMAT_BPP2         = 0x11 << 8,
+    RK_FORMAT_BPP4         = 0x12 << 8,
+    RK_FORMAT_BPP8         = 0x13 << 8,
+
+    RK_FORMAT_Y4           = 0x14 << 8,
+    RK_FORMAT_YCbCr_400    = 0x15 << 8,
+
+    RK_FORMAT_BGRX_8888    = 0x16 << 8,
+
+    RK_FORMAT_YVYU_422     = 0x18 << 8,
+    RK_FORMAT_YVYU_420     = 0x19 << 8,
+    RK_FORMAT_VYUY_422     = 0x1a << 8,
+    RK_FORMAT_VYUY_420     = 0x1b << 8,
+    RK_FORMAT_YUYV_422     = 0x1c << 8,
+    RK_FORMAT_YUYV_420     = 0x1d << 8,
+    RK_FORMAT_UYVY_422     = 0x1e << 8,
+    RK_FORMAT_UYVY_420     = 0x1f << 8,
+
+    RK_FORMAT_YCbCr_420_SP_10B = 0x20 << 8,
+    RK_FORMAT_YCrCb_420_SP_10B = 0x21 << 8,
+    RK_FORMAT_YCbCr_422_SP_10B = 0x22 << 8,
+    RK_FORMAT_YCrCb_422_SP_10B = 0x23 << 8,
+    /* For compatibility with misspellings */
+    RK_FORMAT_YCbCr_422_10b_SP = RK_FORMAT_YCbCr_422_SP_10B,
+    RK_FORMAT_YCrCb_422_10b_SP = RK_FORMAT_YCrCb_422_SP_10B,
+
+    RK_FORMAT_BGR_565      = 0x24 << 8,
+    RK_FORMAT_BGRA_5551    = 0x25 << 8,
+    RK_FORMAT_BGRA_4444    = 0x26 << 8,
+
+    RK_FORMAT_ARGB_8888    = 0x28 << 8,
+    RK_FORMAT_XRGB_8888    = 0x29 << 8,
+    RK_FORMAT_ARGB_5551    = 0x2a << 8,
+    RK_FORMAT_ARGB_4444    = 0x2b << 8,
+    RK_FORMAT_ABGR_8888    = 0x2c << 8,
+    RK_FORMAT_XBGR_8888    = 0x2d << 8,
+    RK_FORMAT_ABGR_5551    = 0x2e << 8,
+    RK_FORMAT_ABGR_4444    = 0x2f << 8,
+
+    RK_FORMAT_RGBA2BPP     = 0x30 << 8,
+
+    RK_FORMAT_UNKNOWN      = 0x100 << 8,
+} RgaSURF_FORMAT;
+
+enum {
+    yuv2rgb_mode0            = 0x0,     /* BT.601 MPEG */
+    yuv2rgb_mode1            = 0x1,     /* BT.601 JPEG */
+    yuv2rgb_mode2            = 0x2,     /* BT.709      */
+
+    rgb2yuv_601_full                = 0x1 << 8,
+    rgb2yuv_709_full                = 0x2 << 8,
+    yuv2yuv_601_limit_2_709_limit   = 0x3 << 8,
+    yuv2yuv_601_limit_2_709_full    = 0x4 << 8,
+    yuv2yuv_709_limit_2_601_limit   = 0x5 << 8,
+    yuv2yuv_709_limit_2_601_full    = 0x6 << 8,     //not support
+    yuv2yuv_601_full_2_709_limit    = 0x7 << 8,
+    yuv2yuv_601_full_2_709_full     = 0x8 << 8,     //not support
+    yuv2yuv_709_full_2_601_limit    = 0x9 << 8,     //not support
+    yuv2yuv_709_full_2_601_full     = 0xa << 8,     //not support
+    full_csc_mask = 0xf00,
+};
+
+enum {
+    RGA3_SCHEDULER_CORE0    = 1 << 0,
+    RGA3_SCHEDULER_CORE1    = 1 << 1,
+    RGA2_SCHEDULER_CORE0    = 1 << 2,
+};
+
+/* RGA3 rd_mode */
+enum
+{
+    raster_mode             = 0x1 << 0,
+    fbc_mode                = 0x1 << 1,
+    tile_mode               = 0x1 << 2,
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /*_RK29_IPP_DRIVER_H_*/

obj_dec/src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

obj_dec/src/libopencv_highgui.so

@@ -0,0 +1 @@
+/usr/local/lib/libopencv_highgui.so

obj_dec/src/rknn_yolov5_demo/CMakeLists.txt

@@ -0,0 +1,80 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(rknn_yolov5_demo)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  message_generation
+)
+
+
+find_package(OpenCV  REQUIRED)
+find_package(Threads REQUIRED)
+
+add_message_files(
+  FILES
+  dis_info.msg
+  dis_info_array.msg
+)
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+
+catkin_package(
+
+ CATKIN_DEPENDS roscpp std_msgs message_runtime
+
+)
+
+
+include_directories(
+include
+  ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  /home/firefly/obj_dec/src/runtime/RK3588/Linux/librknn_api/include
+  /home/firefly/obj_dec/src/3rdparty/rga/RK3588/include
+)
+
+add_library(
+  head 
+  include/rknn_yolov5_demo/detection.h
+  include/rknn_yolov5_demo/postprocess.h
+  include/rknn_yolov5_demo/preprocess.h
+  include/rknn_yolov5_demo/ranging.h
+  include/rknn_yolov5_demo/Timer.h
+  # include/rknn_yolov5_demo/pub_info.h
+  src/detection.cc
+  src/postprocess.cc
+  src/preprocess.cc
+  src/ranging.cc
+  # src/pub_info.cc
+)
+
+add_dependencies(head ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(head
+  ${catkin_LIBRARIES}
+ 
+)
+add_executable(main src/main.cc)
+add_executable(sub_dis src/sub_dis.cc)
+add_dependencies(main ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(sub_dis ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(sub_dis 
+${catkin_LIBRARIES}
+)
+target_link_libraries(main
+head
+${catkin_LIBRARIES}
+${OpenCV_LIBS}
+Threads::Threads
+/home/firefly/obj_dec/src/librknnrt.so
+/home/firefly/obj_dec/src/librga.so
+/home/firefly/obj_dec/src/libopencv_highgui.so
+)
+
+
+
+

obj_dec/src/rknn_yolov5_demo/include/rknn_yolov5_demo/Timer.h

@@ -0,0 +1,33 @@
+/**
+ * @file Timer.h
+ * @author hcy
+ * @brief 用于计时
+ * @version 1
+ * @date 2023-04-10
+ *
+ */
+ 
+ 
+#include <iostream>
+#include <chrono>
+ 
+class Timer
+{
+public:
+    Timer()
+    {
+        start = std::chrono::high_resolution_clock::now();
+    };
+    ~Timer()
+    {
+        end = std::chrono::high_resolution_clock::now();
+        duration = end - start;
+        double ms = duration.count() * 1000.f;
+        std::cout << "Timer took " << ms << "ms" << std::endl;
+    };
+ 
+private:
+    std::chrono::time_point<std::chrono::system_clock> start;
+    std::chrono::time_point<std::chrono::system_clock> end;
+    std::chrono::duration<double> duration;
+};

obj_dec/src/rknn_yolov5_demo/include/rknn_yolov5_demo/detection.h

@@ -0,0 +1,48 @@
+#include <dlfcn.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define _BASETSD_H
+
+#include "rknn_api.h"
+#include "rknn_yolov5_demo/preprocess.h"
+#include "rknn_yolov5_demo/postprocess.h"
+#include "rknn_yolov5_demo/Timer.h"
+
+#define PERF_WITH_POST 1
+
+class Detection
+{
+private:
+
+    int ret;
+    rknn_context ctx;
+    size_t actual_size = 0;
+    int img_width = 0;
+    int img_height = 0;
+    int img_channel = 0;
+    const float nms_threshold = NMS_THRESH;      // 默认的NMS阈值
+    const float box_conf_threshold = BOX_THRESH; // 默认的置信度阈值
+    char* model_name = "/home/firefly/obj_dec/src/rknn_yolov5_demo/model/RK3588/yolov5s-640-640.rknn";
+
+    cv::Mat orig_img;
+    rknn_input_output_num io_num;
+    
+    int channel = 3;
+    int width = 0;
+    int height = 0;
+
+    rknn_input inputs[1];
+    
+    std::vector<float> out_scales;
+    std::vector<int32_t> out_zps;
+    
+public:
+    Detection();
+    static unsigned char *load_data(FILE *fp, size_t ofst, size_t sz);
+    static unsigned char *load_model(const char *filename, int *model_size);
+    detect_result_group_t outputParse(cv::Mat netInputImg);
+
+
+};

obj_dec/src/rknn_yolov5_demo/include/rknn_yolov5_demo/postprocess.h

@@ -0,0 +1,42 @@
+#ifndef _RKNN_YOLOV5_DEMO_POSTPROCESS_H_
+#define _RKNN_YOLOV5_DEMO_POSTPROCESS_H_
+
+#include <stdint.h>
+#include <vector>
+
+#define OBJ_NAME_MAX_SIZE 16
+#define OBJ_NUMB_MAX_SIZE 64
+#define OBJ_CLASS_NUM 80
+#define NMS_THRESH 0.45
+#define BOX_THRESH 0.25
+#define PROP_BOX_SIZE (5 + OBJ_CLASS_NUM)
+
+typedef struct _BOX_RECT
+{
+    int left;
+    int right;
+    int top;
+    int bottom;
+} BOX_RECT;
+
+typedef struct __detect_result_t
+{
+    char name[OBJ_NAME_MAX_SIZE];
+    BOX_RECT box;
+    float prop;
+} detect_result_t;
+
+typedef struct _detect_result_group_t
+{
+    int id;
+    int count;
+    detect_result_t results[OBJ_NUMB_MAX_SIZE];
+} detect_result_group_t;
+
+int post_process(int8_t *input0, int8_t *input1, int8_t *input2, int model_in_h, int model_in_w,
+                 float conf_threshold, float nms_threshold, BOX_RECT pads, float scale_w, float scale_h,
+                 std::vector<int32_t> &qnt_zps, std::vector<float> &qnt_scales,
+                 detect_result_group_t *group);
+
+void deinitPostProcess();
+#endif //_RKNN_YOLOV5_DEMO_POSTPROCESS_H_

obj_dec/src/rknn_yolov5_demo/include/rknn_yolov5_demo/preprocess.h

@@ -0,0 +1,16 @@
+#ifndef _RKNN_YOLOV5_DEMO_PREPROCESS_H_
+#define _RKNN_YOLOV5_DEMO_PREPROCESS_H_
+
+#include <stdio.h>
+#include "im2d.h"
+#include "rga.h"
+#include "opencv2/core/core.hpp"
+#include "opencv2/imgcodecs.hpp"
+#include "opencv2/imgproc.hpp"
+#include "rknn_yolov5_demo/postprocess.h"
+
+void letterbox(const cv::Mat &image, cv::Mat &padded_image, BOX_RECT &pads, const float scale, const cv::Size &target_size, const cv::Scalar &pad_color = cv::Scalar(128, 128, 128));
+
+int resize_rga(rga_buffer_t &src, rga_buffer_t &dst, const cv::Mat &image, cv::Mat &resized_image, const cv::Size &target_size);
+
+#endif //_RKNN_YOLOV5_DEMO_PREPROCESS_H_

obj_dec/src/rknn_yolov5_demo/include/rknn_yolov5_demo/pub_info.h

@@ -0,0 +1,32 @@
+#ifndef PUB_INFO_H
+#define PUB_INFO_H
+#include <ros/ros.h>
+#include <thread>
+#include "rknn_yolov5_demo/dis_info.h"
+#include "rknn_yolov5_demo/dis_info_array.h"
+#include "rknn_yolov5_demo/ranging.h"
+
+class Pub_info{
+    public:
+    Pub_info(){
+        dis_pub_ = nh_.advertise<rknn_yolov5_demo::dis_info_array>("ceju_info",10);
+        thread_1 = std::thread(&Pub_info::pub_dis,this);
+        // thread_2 = std::thread(&Pub_info::)
+
+    };
+    ~Pub_info(){
+        thread_1.join();
+    }
+    void pub_dis();
+    public:
+    ros::NodeHandle nh_;
+    ros::Publisher dis_pub_;
+    rknn_yolov5_demo::dis_info_array dis_array_;
+    rknn_yolov5_demo::dis_info data;
+    std::thread thread_1;
+    // std::thread thread_2;
+
+    
+
+};
+#endif

obj_dec/src/rknn_yolov5_demo/include/rknn_yolov5_demo/ranging.h

@@ -0,0 +1,80 @@
+#pragma once
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/calib3d.hpp>
+
+#include <cstdlib>
+#include <vector>
+#include "rknn_yolov5_demo/detection.h"
+#include <unistd.h>
+
+
+using namespace cv;
+
+class Ranging
+{
+private:
+    // rknn_handle hdx;
+    cv::VideoCapture vcapture;
+    Detection yolov5s;
+   
+   /* new stereo
+    Mat cam_matrix_left = (Mat_<double>(3, 3) << 
+        4.809895643547006e+02, 0, 0,
+        0,4.807599168204821e+02, 0,
+        3.362108165786334e+02, 2.298502481932070e+02, 1);
+    Mat cam_matrix_right = (Mat_<double>(3, 3) << 
+        4.903260126250775e+02, 0, 0,
+        0,4.900310486342847e+02, 0,
+        3.230124997386542e+02, 2.346297967642670e+02, 1);
+    Mat distortion_l = (Mat_<double>(1, 5) <<0.113688825569154,-0.106166584327678, 0,
+        0, 0);
+
+    Mat distortion_r = (Mat_<double>(1, 5) <<0.121425307936153,-0.141892782717707, 0,
+        0, 0);
+    Mat rotate = (Mat_<double>(3, 3) << 
+        0.999996295879846, 8.723884080433472e-04, 0.002578209660240,
+        -8.682590894537506e-04,0.999998339366207, -0.001602308016931,
+        -0.002579603213718,0.001600063527818,0.999995392711370);
+    Mat trans = (Mat_<double>(3, 1) << 
+         -60.348359844102470,-0.030699794141365, 0.495248628081046);
+*/
+    Mat cam_matrix_left = (Mat_<double>(3, 3) << 
+    4.869084743604942e+02, 0, 0,
+    0,4.859921620798602e+02, 0,
+    2.813183643903652e+02, 2.267657091677993e+02, 1);
+    Mat cam_matrix_right = (Mat_<double>(3, 3) << 
+    4.859133331805883e+02, 0, 0,
+    0,4.850356748771951e+02, 0,
+    2.970046483040089e+02, 2.324763397214774e+02, 1);
+    Mat distortion_l = (Mat_<double>(1, 5) <<0.121235284781974,-0.161097849662596, 0,
+        0, 0);
+
+    Mat distortion_r = (Mat_<double>(1, 5) <<0.105479235005840,-0.120347246815955, 0,
+        0, 0);
+    Mat rotate = (Mat_<double>(3, 3) << 
+    0.999921984818601, 3.753847738839353e-04, -0.012485325894835,
+    -4.085449515452996e-04, 0.999996396040715, -0.002653487630467,
+    0.012484284819374, 0.002658381435011,0.999918534502034);
+    Mat trans = (Mat_<double>(3, 1) << 
+       -60.319997608188590, -0.019664800882533, -0.638993708428792);
+
+    cv::Mat mapX1, mapX2, mapY1, mapY2, q, Z;
+
+    int imgw, imgh;
+    detect_result_group_t detect_result_group;
+    cv::Mat info;
+    cv::Mat empty;
+
+  
+
+public:
+    Ranging(){};
+    Ranging(int index, int imgw, int imgh);
+    void rectifyImage(cv::Mat &oriImgL, cv::Mat &oriImgR);
+    void getInfo(cv::Mat &imgl, cv::Mat &imgr, cv::Mat &detBoxes, cv::Mat &info);
+    std::vector<float> pic2cam(int u, int v);
+    std::vector<int> muban(cv::Mat &left_image, cv::Mat &right_image, const int *coordinates);
+    std::vector<cv::Mat> get_range();
+    void horizon_estimate(cv::Mat& img, cv::Mat& bboxs,int k);
+};

obj_dec/src/rknn_yolov5_demo/model/coco_80_labels_list.txt

@@ -0,0 +1,80 @@
+person
+bicycle
+car
+motorcycle
+airplane
+bus
+train
+truck
+boat
+traffic light
+fire hydrant
+stop sign
+parking meter
+bench
+bird
+cat
+dog
+horse
+sheep
+cow
+elephant
+bear
+zebra
+giraffe
+backpack
+umbrella
+handbag
+tie
+suitcase
+frisbee
+skis
+snowboard
+sports ball
+kite
+baseball bat
+baseball glove
+skateboard
+surfboard
+tennis racket
+bottle
+wine glass
+cup
+fork
+knife
+spoon
+bowl
+banana
+apple
+sandwich
+orange
+broccoli
+carrot
+hot dog
+pizza
+donut
+cake
+chair
+couch
+potted plant
+bed
+dining table
+toilet
+tv
+laptop
+mouse
+remote
+keyboard
+cell phone
+microwave
+oven
+toaster
+sink
+refrigerator
+book
+clock
+vase
+scissors
+teddy bear
+hair drier
+toothbrush

obj_dec/src/rknn_yolov5_demo/msg/dis_info.msg

@@ -0,0 +1,4 @@
+float32 distance
+float32 width
+float32 height
+float32 angle

obj_dec/src/rknn_yolov5_demo/msg/dis_info_array.msg

@@ -0,0 +1 @@
+rknn_yolov5_demo/dis_info[] dis

obj_dec/src/rknn_yolov5_demo/package.xml

@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rknn_yolov5_demo</name>
+  <version>0.0.0</version>
+  <description>The rknn_yolov5_demo package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="firefly@todo.todo">firefly</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rknn_yolov5_demo</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>ros_py</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>ros_py</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>ros_py</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

obj_dec/src/rknn_yolov5_demo/src/detection.cc

@@ -0,0 +1,201 @@
+#include "rknn_yolov5_demo/detection.h"
+#include <opencv2/opencv.hpp>
+
+
+unsigned char * Detection::load_data(FILE *fp, size_t ofst, size_t sz)
+{
+  unsigned char *data;
+  int ret;
+
+  data = NULL;
+
+  if (NULL == fp)
+  {
+    return NULL;
+  }
+
+  ret = fseek(fp, ofst, SEEK_SET);
+  if (ret != 0)
+  {
+    printf("blob seek failure.\n");
+    return NULL;
+  }
+
+  data = (unsigned char *)malloc(sz);
+  if (data == NULL)
+  {
+    printf("buffer malloc failure.\n");
+    return NULL;
+  }
+  ret = fread(data, 1, sz, fp);
+  return data;
+}
+
+unsigned char * Detection::load_model(const char *filename, int *model_size)
+{
+  FILE *fp;
+  unsigned char *data;
+
+  fp = fopen(filename, "rb");
+  if (NULL == fp)
+  {
+    printf("Open file %s failed.\n", filename);
+    return NULL;
+  }
+
+  fseek(fp, 0, SEEK_END);
+  int size = ftell(fp);
+
+  data = load_data(fp, 0, size);
+
+  fclose(fp);
+
+  *model_size = size;
+  return data;
+}
+
+Detection::Detection()
+{
+  
+    /* Create the neural network */
+    printf("Loading mode...\n");
+    int model_data_size = 0;
+    unsigned char *model_data = load_model(model_name, &model_data_size);
+    ret = rknn_init(&ctx, model_data, model_data_size, 0, NULL);
+    if (ret < 0)
+    {
+      printf("rknn_init error ret=%d\n", ret);
+    }
+
+    rknn_sdk_version version;
+    ret = rknn_query(ctx, RKNN_QUERY_SDK_VERSION, &version, sizeof(rknn_sdk_version));
+    if (ret < 0)
+    {
+      printf("rknn_init error ret=%d\n", ret);
+    }
+    printf("sdk version: %s driver version: %s\n", version.api_version, version.drv_version);
+
+    ret = rknn_query(ctx, RKNN_QUERY_IN_OUT_NUM, &io_num, sizeof(io_num));
+    if (ret < 0)
+    {
+      printf("rknn_init error ret=%d\n", ret);
+    }
+    printf("model input num: %d, output num: %d\n", io_num.n_input, io_num.n_output);
+
+    rknn_tensor_attr input_attrs[io_num.n_input];
+    memset(input_attrs, 0, sizeof(input_attrs));
+    for (int i = 0; i < io_num.n_input; i++)
+    {
+      input_attrs[i].index = i;
+      ret = rknn_query(ctx, RKNN_QUERY_INPUT_ATTR, &(input_attrs[i]), sizeof(rknn_tensor_attr));
+      if (ret < 0)
+      {
+        printf("rknn_init error ret=%d\n", ret);
+      }
+      // dump_tensor_attr(&(input_attrs[i]));
+    }
+
+    rknn_tensor_attr output_attrs[io_num.n_output];
+    memset(output_attrs, 0, sizeof(output_attrs));
+    for (int i = 0; i < io_num.n_output; i++)
+    {
+      output_attrs[i].index = i;
+      ret = rknn_query(ctx, RKNN_QUERY_OUTPUT_ATTR, &(output_attrs[i]), sizeof(rknn_tensor_attr));
+      // dump_tensor_attr(&(output_attrs[i]));
+    }
+
+    for (int i = 0; i < io_num.n_output; ++i)
+    {
+      out_scales.push_back(output_attrs[i].scale);
+      out_zps.push_back(output_attrs[i].zp);
+    }
+    
+    if (input_attrs[0].fmt == RKNN_TENSOR_NCHW)
+    {
+      printf("model is NCHW input fmt\n");
+      channel = input_attrs[0].dims[1];
+      height = input_attrs[0].dims[2];
+      width = input_attrs[0].dims[3];
+    }
+    else
+    {
+      printf("model is NHWC input fmt\n");
+      height = input_attrs[0].dims[1];
+      width = input_attrs[0].dims[2];
+      channel = input_attrs[0].dims[3];
+    }
+
+    printf("model input height=%d, width=%d, channel=%d\n", height, width, channel);
+
+    
+    memset(inputs, 0, sizeof(inputs));
+    inputs[0].index = 0;
+    inputs[0].type = RKNN_TENSOR_UINT8;
+    inputs[0].size = width * height * channel;
+    inputs[0].fmt = RKNN_TENSOR_NHWC;
+    inputs[0].pass_through = 0;
+    
+  }
+
+detect_result_group_t Detection::outputParse(cv::Mat netInputImg)
+{
+
+    orig_img = netInputImg.clone();
+
+    if (!orig_img.data)
+    {
+      printf("no image\n");
+    }
+    cv::Mat img;
+    cv::cvtColor(orig_img, img, cv::COLOR_BGR2RGB);
+    img_width = img.cols;
+    img_height = img.rows;
+    // printf("img width = %d, img height = %d\n", img_width, img_height);
+
+    // 指定目标大小和预处理方式,默认使用LetterBox的预处理
+    BOX_RECT pads;
+    memset(&pads, 0, sizeof(BOX_RECT));
+    cv::Size target_size(width, height);
+    cv::Mat resized_img(target_size.height, target_size.width, CV_8UC3);
+    // 计算缩放比例
+    float scale_w = (float)target_size.width / img.cols;
+    float scale_h = (float)target_size.height / img.rows;
+
+    if (img_width != width || img_height != height)
+    {
+      // printf("resize image with letterbox\n");
+      float min_scale = std::min(scale_w, scale_h);
+      scale_w = min_scale;
+      scale_h = min_scale;
+      letterbox(img, resized_img, pads, min_scale, target_size);
+      // 保存预处理图片
+      // cv::imwrite("letterbox_input.jpg", resized_img);
+      inputs[0].buf = resized_img.data;
+    }
+    else
+    {
+      inputs[0].buf = img.data;
+    }
+
+    rknn_inputs_set(ctx, io_num.n_input, inputs);
+
+    rknn_output outputs[io_num.n_output];
+    memset(outputs, 0, sizeof(outputs));
+    for (int i = 0; i < io_num.n_output; i++)
+    {
+      outputs[i].want_float = 0;
+    }
+
+    // 执行推理
+    ret = rknn_run(ctx, NULL);
+    ret = rknn_outputs_get(ctx, io_num.n_output, outputs, NULL);
+
+    // 后处理
+    detect_result_group_t detect_result_group;
+    
+    post_process((int8_t *)outputs[0].buf, (int8_t *)outputs[1].buf, (int8_t *)outputs[2].buf, height, width,
+                box_conf_threshold, nms_threshold, pads, scale_w, scale_h, out_zps, out_scales, &detect_result_group);
+
+
+    return detect_result_group;
+}

obj_dec/src/rknn_yolov5_demo/src/main.cc

@@ -0,0 +1,84 @@
+#include <iostream>
+#include <stdio.h>
+#include <ros/ros.h>
+#include <opencv2/opencv.hpp>
+#include "rknn_yolov5_demo/ranging.h"
+#include <string>
+#include <pthread.h>
+#include <mutex>
+#include <sys/time.h>
+#include "opencv2/imgcodecs/imgcodecs.hpp"
+#include <ctime>
+#include <queue>
+#include "rknn_yolov5_demo/dis_info.h"
+#include "rknn_yolov5_demo/dis_info_array.h"
+
+#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
+
+Ranging r(11, 640, 480);
+std::queue<std::vector<cv::Mat>> frameInfo;
+std::mutex g_mutex;
+
+void *ranging(void *args)		// ranging线程
+{
+	ros::Publisher dis_pub_;
+	ros::NodeHandle nh; 
+    dis_pub_ = nh.advertise<rknn_yolov5_demo::dis_info_array>("ceju_info",10);
+	while (true)
+	{
+		// std::cout<<" ************ enter ranging *********** "<<std::endl;
+		std::vector<cv::Mat> result = r.get_range();
+		cv ::Mat info = result[2];
+		rknn_yolov5_demo::dis_info_array dis_array;
+		if(!info.empty())
+		{
+			for(int i=0;i<info.rows;i++)
+			{
+				// std::cout<<"1"<<std::endl;
+				if(info.at<float>(i,0)>0&&info.at<float>(i,0)<200)
+				{
+					rknn_yolov5_demo::dis_info data;
+					data.distance = info.at<float>(i,0);
+					data.width = info.at<float>(i,1);
+					data.height = info.at<float>(i,2);
+					data.angle = info.at<float>(i,3);
+					dis_array.dis.push_back(data);
+					std::cout<<data.distance<<std::endl;
+				}	
+			}
+			
+        dis_pub_.publish(dis_array);
+		// if (!dis_array.dis.empty())
+		// {
+		// 	for (const auto& data : dis_array.dis)
+		// 	{
+		// 		ROS_INFO("distance: %.2f, width: %.2f, height: %.2f, angle: %.2f",
+		// 				data.distance, data.width, data.height, data.angle);
+		// 	}
+		// }
+
+		
+		}
+		// else{
+		// 	 std::cerr << "Info matrix is empty!" << std::endl;
+		// }
+		g_mutex.lock();
+		for (uchar i = 0; i < frameInfo.size(); i++)		// 只保存当前最新的图片帧信息
+		frameInfo.pop();
+		frameInfo.push(result);
+		g_mutex.unlock();
+	}
+	
+	}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "stereo");
+	ros::NodeHandle nh; 
+	pthread_t tids[1];		// 执行ranging线程
+	int ret = pthread_create(&tids[0], NULL, ranging, NULL);
+	pthread_join(tids[0],NULL);
+
+	
+	return 0;
+}

obj_dec/src/rknn_yolov5_demo/src/postprocess.cc

@@ -0,0 +1,373 @@
+// Copyright (c) 2021 by Rockchip Electronics Co., Ltd. All Rights Reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rknn_yolov5_demo/postprocess.h"
+
+#include <math.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/time.h>
+#include <set>
+#include <vector>
+#define LABEL_NALE_TXT_PATH "/home/firefly/obj_dec/src/rknn_yolov5_demo/model/coco_80_labels_list.txt"
+
+static char *labels[OBJ_CLASS_NUM];
+
+const int anchor0[6] = {10, 13, 16, 30, 33, 23};
+const int anchor1[6] = {30, 61, 62, 45, 59, 119};
+const int anchor2[6] = {116, 90, 156, 198, 373, 326};
+
+inline static int clamp(float val, int min, int max) { return val > min ? (val < max ? val : max) : min; }
+
+char *readLine(FILE *fp, char *buffer, int *len)
+{
+  int ch;
+  int i = 0;
+  size_t buff_len = 0;
+
+  buffer = (char *)malloc(buff_len + 1);
+  if (!buffer)
+    return NULL; // Out of memory
+
+  while ((ch = fgetc(fp)) != '\n' && ch != EOF)
+  {
+    buff_len++;
+    void *tmp = realloc(buffer, buff_len + 1);
+    if (tmp == NULL)
+    {
+      free(buffer);
+      return NULL; // Out of memory
+    }
+    buffer = (char *)tmp;
+
+    buffer[i] = (char)ch;
+    i++;
+  }
+  buffer[i] = '\0';
+
+  *len = buff_len;
+
+  // Detect end
+  if (ch == EOF && (i == 0 || ferror(fp)))
+  {
+    free(buffer);
+    return NULL;
+  }
+  return buffer;
+}
+
+int readLines(const char *fileName, char *lines[], int max_line)
+{
+  FILE *file = fopen(fileName, "r");
+  char *s;
+  int i = 0;
+  int n = 0;
+
+  if (file == NULL)
+  {
+    printf("Open %s fail!\n", fileName);
+    return -1;
+  }
+
+  while ((s = readLine(file, s, &n)) != NULL)
+  {
+    lines[i++] = s;
+    if (i >= max_line)
+      break;
+  }
+  fclose(file);
+  return i;
+}
+
+int loadLabelName(const char *locationFilename, char *label[])
+{
+  printf("loadLabelName %s\n", locationFilename);
+  readLines(locationFilename, label, OBJ_CLASS_NUM);
+  return 0;
+}
+
+static float CalculateOverlap(float xmin0, float ymin0, float xmax0, float ymax0, float xmin1, float ymin1, float xmax1,
+                              float ymax1)
+{
+  float w = fmax(0.f, fmin(xmax0, xmax1) - fmax(xmin0, xmin1) + 1.0);
+  float h = fmax(0.f, fmin(ymax0, ymax1) - fmax(ymin0, ymin1) + 1.0);
+  float i = w * h;
+  float u = (xmax0 - xmin0 + 1.0) * (ymax0 - ymin0 + 1.0) + (xmax1 - xmin1 + 1.0) * (ymax1 - ymin1 + 1.0) - i;
+  return u <= 0.f ? 0.f : (i / u);
+}
+
+static int nms(int validCount, std::vector<float> &outputLocations, std::vector<int> classIds, std::vector<int> &order,
+               int filterId, float threshold)
+{
+  for (int i = 0; i < validCount; ++i)
+  {
+    if (order[i] == -1 || classIds[i] != filterId)
+    {
+      continue;
+    }
+    int n = order[i];
+    for (int j = i + 1; j < validCount; ++j)
+    {
+      int m = order[j];
+      if (m == -1 || classIds[i] != filterId)
+      {
+        continue;
+      }
+      float xmin0 = outputLocations[n * 4 + 0];
+      float ymin0 = outputLocations[n * 4 + 1];
+      float xmax0 = outputLocations[n * 4 + 0] + outputLocations[n * 4 + 2];
+      float ymax0 = outputLocations[n * 4 + 1] + outputLocations[n * 4 + 3];
+
+      float xmin1 = outputLocations[m * 4 + 0];
+      float ymin1 = outputLocations[m * 4 + 1];
+      float xmax1 = outputLocations[m * 4 + 0] + outputLocations[m * 4 + 2];
+      float ymax1 = outputLocations[m * 4 + 1] + outputLocations[m * 4 + 3];
+
+      float iou = CalculateOverlap(xmin0, ymin0, xmax0, ymax0, xmin1, ymin1, xmax1, ymax1);
+
+      if (iou > threshold)
+      {
+        order[j] = -1;
+      }
+    }
+  }
+  return 0;
+}
+
+static int quick_sort_indice_inverse(std::vector<float> &input, int left, int right, std::vector<int> &indices)
+{
+  float key;
+  int key_index;
+  int low = left;
+  int high = right;
+  if (left < right)
+  {
+    key_index = indices[left];
+    key = input[left];
+    while (low < high)
+    {
+      while (low < high && input[high] <= key)
+      {
+        high--;
+      }
+      input[low] = input[high];
+      indices[low] = indices[high];
+      while (low < high && input[low] >= key)
+      {
+        low++;
+      }
+      input[high] = input[low];
+      indices[high] = indices[low];
+    }
+    input[low] = key;
+    indices[low] = key_index;
+    quick_sort_indice_inverse(input, left, low - 1, indices);
+    quick_sort_indice_inverse(input, low + 1, right, indices);
+  }
+  return low;
+}
+
+static float sigmoid(float x) { return 1.0 / (1.0 + expf(-x)); }
+
+static float unsigmoid(float y) { return -1.0 * logf((1.0 / y) - 1.0); }
+
+inline static int32_t __clip(float val, float min, float max)
+{
+  float f = val <= min ? min : (val >= max ? max : val);
+  return f;
+}
+
+static int8_t qnt_f32_to_affine(float f32, int32_t zp, float scale)
+{
+  float dst_val = (f32 / scale) + zp;
+  int8_t res = (int8_t)__clip(dst_val, -128, 127);
+  return res;
+}
+
+static float deqnt_affine_to_f32(int8_t qnt, int32_t zp, float scale) { return ((float)qnt - (float)zp) * scale; }
+
+static int process(int8_t *input, int *anchor, int grid_h, int grid_w, int height, int width, int stride,
+                   std::vector<float> &boxes, std::vector<float> &objProbs, std::vector<int> &classId, float threshold,
+                   int32_t zp, float scale)
+{
+  int validCount = 0;
+  int grid_len = grid_h * grid_w;
+  int8_t thres_i8 = qnt_f32_to_affine(threshold, zp, scale);
+  for (int a = 0; a < 3; a++)
+  {
+    for (int i = 0; i < grid_h; i++)
+    {
+      for (int j = 0; j < grid_w; j++)
+      {
+        int8_t box_confidence = input[(PROP_BOX_SIZE * a + 4) * grid_len + i * grid_w + j];
+        if (box_confidence >= thres_i8)
+        {
+          int offset = (PROP_BOX_SIZE * a) * grid_len + i * grid_w + j;
+          int8_t *in_ptr = input + offset;
+          float box_x = (deqnt_affine_to_f32(*in_ptr, zp, scale)) * 2.0 - 0.5;
+          float box_y = (deqnt_affine_to_f32(in_ptr[grid_len], zp, scale)) * 2.0 - 0.5;
+          float box_w = (deqnt_affine_to_f32(in_ptr[2 * grid_len], zp, scale)) * 2.0;
+          float box_h = (deqnt_affine_to_f32(in_ptr[3 * grid_len], zp, scale)) * 2.0;
+          box_x = (box_x + j) * (float)stride;
+          box_y = (box_y + i) * (float)stride;
+          box_w = box_w * box_w * (float)anchor[a * 2];
+          box_h = box_h * box_h * (float)anchor[a * 2 + 1];
+          box_x -= (box_w / 2.0);
+          box_y -= (box_h / 2.0);
+
+          int8_t maxClassProbs = in_ptr[5 * grid_len];
+          int maxClassId = 0;
+          for (int k = 1; k < OBJ_CLASS_NUM; ++k)
+          {
+            int8_t prob = in_ptr[(5 + k) * grid_len];
+            if (prob > maxClassProbs)
+            {
+              maxClassId = k;
+              maxClassProbs = prob;
+            }
+          }
+          if (maxClassProbs > thres_i8)
+          {
+            objProbs.push_back((deqnt_affine_to_f32(maxClassProbs, zp, scale)) * (deqnt_affine_to_f32(box_confidence, zp, scale)));
+            classId.push_back(maxClassId);
+            validCount++;
+            boxes.push_back(box_x);
+            boxes.push_back(box_y);
+            boxes.push_back(box_w);
+            boxes.push_back(box_h);
+          }
+        }
+      }
+    }
+  }
+  return validCount;
+}
+
+int post_process(int8_t *input0, int8_t *input1, int8_t *input2, int model_in_h, int model_in_w, float conf_threshold,
+                 float nms_threshold, BOX_RECT pads, float scale_w, float scale_h, std::vector<int32_t> &qnt_zps,
+                 std::vector<float> &qnt_scales, detect_result_group_t *group)
+{
+  static int init = -1;
+  if (init == -1)
+  {
+    int ret = 0;
+    ret = loadLabelName(LABEL_NALE_TXT_PATH, labels);
+    if (ret < 0)
+    {
+      return -1;
+    }
+
+    init = 0;
+  }
+  memset(group, 0, sizeof(detect_result_group_t));
+
+  std::vector<float> filterBoxes;
+  std::vector<float> objProbs;
+  std::vector<int> classId;
+
+  // stride 8
+  int stride0 = 8;
+  int grid_h0 = model_in_h / stride0;
+  int grid_w0 = model_in_w / stride0;
+  int validCount0 = 0;
+  validCount0 = process(input0, (int *)anchor0, grid_h0, grid_w0, model_in_h, model_in_w, stride0, filterBoxes, objProbs,
+                        classId, conf_threshold, qnt_zps[0], qnt_scales[0]);
+
+  // stride 16
+  int stride1 = 16;
+  int grid_h1 = model_in_h / stride1;
+  int grid_w1 = model_in_w / stride1;
+  int validCount1 = 0;
+  validCount1 = process(input1, (int *)anchor1, grid_h1, grid_w1, model_in_h, model_in_w, stride1, filterBoxes, objProbs,
+                        classId, conf_threshold, qnt_zps[1], qnt_scales[1]);
+
+  // stride 32
+  int stride2 = 32;
+  int grid_h2 = model_in_h / stride2;
+  int grid_w2 = model_in_w / stride2;
+  int validCount2 = 0;
+  validCount2 = process(input2, (int *)anchor2, grid_h2, grid_w2, model_in_h, model_in_w, stride2, filterBoxes, objProbs,
+                        classId, conf_threshold, qnt_zps[2], qnt_scales[2]);
+
+  int validCount = validCount0 + validCount1 + validCount2;
+  // no object detect
+  if (validCount <= 0)
+  {
+    return 0;
+  }
+
+  std::vector<int> indexArray;
+  for (int i = 0; i < validCount; ++i)
+  {
+    indexArray.push_back(i);
+  }
+
+  quick_sort_indice_inverse(objProbs, 0, validCount - 1, indexArray);
+
+  std::set<int> class_set(std::begin(classId), std::end(classId));
+
+  for (auto c : class_set)
+  {
+    nms(validCount, filterBoxes, classId, indexArray, c, nms_threshold);
+  }
+
+  int last_count = 0;
+  group->count = 0;
+  /* box valid detect target */
+  for (int i = 0; i < validCount; ++i)
+  {
+    if (indexArray[i] == -1 || last_count >= OBJ_NUMB_MAX_SIZE)
+    {
+      continue;
+    }
+    int n = indexArray[i];
+
+    float x1 = filterBoxes[n * 4 + 0] - pads.left;
+    float y1 = filterBoxes[n * 4 + 1] - pads.top;
+    float x2 = x1 + filterBoxes[n * 4 + 2];
+    float y2 = y1 + filterBoxes[n * 4 + 3];
+    int id = classId[n];
+    float obj_conf = objProbs[i];
+
+    group->results[last_count].box.left = (int)(clamp(x1, 0, model_in_w) / scale_w);
+    group->results[last_count].box.top = (int)(clamp(y1, 0, model_in_h) / scale_h);
+    group->results[last_count].box.right = (int)(clamp(x2, 0, model_in_w) / scale_w);
+    group->results[last_count].box.bottom = (int)(clamp(y2, 0, model_in_h) / scale_h);
+    group->results[last_count].prop = obj_conf;
+    char *label = labels[id];
+    strncpy(group->results[last_count].name, label, OBJ_NAME_MAX_SIZE);
+
+    // printf("result %2d: (%4d, %4d, %4d, %4d), %s\n", i, group->results[last_count].box.left,
+    // group->results[last_count].box.top,
+    //        group->results[last_count].box.right, group->results[last_count].box.bottom, label);
+    last_count++;
+  }
+  group->count = last_count;
+
+  return 0;
+}
+
+void deinitPostProcess()
+{
+  for (int i = 0; i < OBJ_CLASS_NUM; i++)
+  {
+    if (labels[i] != nullptr)
+    {
+      free(labels[i]);
+      labels[i] = nullptr;
+    }
+  }
+}

obj_dec/src/rknn_yolov5_demo/src/preprocess.cc

@@ -0,0 +1,61 @@
+// Copyright (c) 2023 by Rockchip Electronics Co., Ltd. All Rights Reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rknn_yolov5_demo/preprocess.h"
+
+void letterbox(const cv::Mat &image, cv::Mat &padded_image, BOX_RECT &pads, const float scale, const cv::Size &target_size, const cv::Scalar &pad_color)
+{
+    // 调整图像大小
+    cv::Mat resized_image;
+    cv::resize(image, resized_image, cv::Size(), scale, scale);
+
+    // 计算填充大小
+    int pad_width = target_size.width - resized_image.cols;
+    int pad_height = target_size.height - resized_image.rows;
+
+    pads.left = pad_width / 2;
+    pads.right = pad_width - pads.left;
+    pads.top = pad_height / 2;
+    pads.bottom = pad_height - pads.top;
+
+    // 在图像周围添加填充
+    cv::copyMakeBorder(resized_image, padded_image, pads.top, pads.bottom, pads.left, pads.right, cv::BORDER_CONSTANT, pad_color);
+}
+
+int resize_rga(rga_buffer_t &src, rga_buffer_t &dst, const cv::Mat &image, cv::Mat &resized_image, const cv::Size &target_size)
+{
+    im_rect src_rect;
+    im_rect dst_rect;
+    memset(&src_rect, 0, sizeof(src_rect));
+    memset(&dst_rect, 0, sizeof(dst_rect));
+    size_t img_width = image.cols;
+    size_t img_height = image.rows;
+    if (image.type() != CV_8UC3)
+    {
+        printf("source image type is %d!\n", image.type());
+        return -1;
+    }
+    size_t target_width = target_size.width;
+    size_t target_height = target_size.height;
+    src = wrapbuffer_virtualaddr((void *)image.data, img_width, img_height, RK_FORMAT_RGB_888);
+    dst = wrapbuffer_virtualaddr((void *)resized_image.data, target_width, target_height, RK_FORMAT_RGB_888);
+    int ret = imcheck(src, dst, src_rect, dst_rect);
+    if (IM_STATUS_NOERROR != ret)
+    {
+        fprintf(stderr, "rga check error! %s", imStrError((IM_STATUS)ret));
+        return -1;
+    }
+    IM_STATUS STATUS = imresize(src, dst);
+    return 0;
+}

obj_dec/src/rknn_yolov5_demo/src/pub_info.cc

@@ -0,0 +1,28 @@
+#include "rknn_yolov5_demo/pub_info.h"
+
+
+void Pub_info::pub_dis()
+{
+    Ranging ranging;
+    ros::Rate loop_rate(10);
+
+    while(ros::ok()){  
+        std::vector<cv::Mat> result = ranging.get_range();
+        cv::Mat info = result[2];
+        for(int i=0;i<info.rows;i++)
+        {
+            data.distance = info.at<float>(i,0);
+            data.width = info.at<float>(i,1);
+            data.height = info.at<float>(i,2);
+            data.angle = info.at<float>(i,3);
+            dis_array_.dis.push_back(data);
+        }
+        dis_pub_.publish(dis_array_);
+        loop_rate.sleep();
+
+    }
+ 
+    
+
+}
+

obj_dec/src/rknn_yolov5_demo/src/ranging.cc

@@ -0,0 +1,399 @@
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui.hpp>
+#include <iostream>
+#include <stdio.h>
+#include "rknn_yolov5_demo/ranging.h"
+#include <math.h>
+#include <stdlib.h>
+
+#include <string.h>
+#include <sys/time.h>
+#include <dlfcn.h>
+#include "opencv2/core.hpp"
+#include "opencv2/imgcodecs.hpp"
+#include "opencv2/imgproc.hpp"
+
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <dirent.h>
+#include <vector>
+#include <tuple>
+#include <string>
+#include <string.h>
+#include <algorithm>
+// #include <ros/ros.h>
+
+
+using namespace cv;
+void Ranging::rectifyImage(Mat &oriImgL, Mat &oriImgR) //重映射函数
+{
+    remap(oriImgL, oriImgL, mapX1, mapX2, cv::INTER_LINEAR);
+    remap(oriImgR, oriImgR, mapY1, mapY2, cv::INTER_LINEAR);
+}
+
+std::vector<float> Ranging::pic2cam(int u, int v) //像素坐标转相机坐标
+{
+    //(u,v)->(x,y)"(loc[0],loc[1])"
+    std::vector<float> loc;
+    loc.push_back((u - cam_matrix_right.at<double>(0, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(0, 0));
+    loc.push_back((v - cam_matrix_right.at<double>(1, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(1, 1));
+    return loc;
+}
+
+std::vector<int> Ranging::muban(Mat &left_image, Mat &right_image, const int *coordinates) //模板匹配
+{
+    int x1 = coordinates[0], y1 = coordinates[1], x2 = coordinates[2], y2 = coordinates[3];
+    Mat tpl = right_image.rowRange(max(y1 - 2, 0), min(y2 + 2, 479)).colRange(x1, x2); //获取目标框
+    Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639); //待匹配图像，极线约束，只需要同水平区域
+    int th = tpl.rows, tw = tpl.cols;
+    Mat result;
+
+    matchTemplate(target, tpl, result, TM_CCOEFF_NORMED); //匹配方法：归一化相关系数即零均值归一化互相关
+    double minVal, maxVal;
+    Point minLoc, maxLoc; 
+    minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc); //得到匹配点坐标
+    Point tl = maxLoc, br;
+
+    br.x = min(maxLoc.x + tw, 639); //转为像素坐标系
+    br.y = min(maxLoc.y + th, 479); //转为像素坐标系
+    ////展示匹配结果
+    // br.x = min(maxLoc.x + tw, 319);
+    // br.y = min(maxLoc.y + th, 239);
+    //rectangle(target, tl, br, (0, 255, 0), 3);
+    //imshow("match-", target);
+    //waitKey(2);
+
+    std::vector<int> maxloc;
+    maxloc.push_back(maxLoc.x); 
+    maxloc.push_back(maxLoc.y); 
+    return maxloc;
+}
+
+void Ranging::horizon_estimate(Mat& img, Mat& bboxs,int k)
+{
+    //保证摄像头与地面平行
+	int x1 = bboxs.at<float>(k, 0);
+	int x2 = bboxs.at<float>(k, 2);
+	int y1 = bboxs.at<float>(k, 1);
+	int y2 = bboxs.at<float>(k, 3);
+    float Conf = bboxs.at<float>(k, 4);
+    int cls = bboxs.at<float>(k, 5);
+	float Y_B, Y_H;
+	cv::Mat edge, grayImage;
+	std::vector<cv::Point> idx;
+    cv::Mat tpl = img.rowRange(y1, min(y2+5,479)).colRange(x1, x2); // 取感兴趣范围
+    //Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639);
+	cv::Mat Z = Mat::zeros(2, tpl.cols, CV_32FC1);
+	cvtColor(tpl, grayImage, COLOR_BGR2GRAY); 
+	GaussianBlur(grayImage,grayImage,Size(5,5),0);
+	Canny(grayImage, edge, 120, 180, 3);  //提取边缘，获取与地面接触点
+    //cv::imshow("1",edge);
+    //cv::waitKey(1);
+    float cluster[650];
+	for (int i = 0;i<650;i++)
+	{
+	    cluster[i] = 0;
+	}
+	int y_b, y_h;
+    int j = 0;
+	for (int i = 0; i < x2-x1; i++)
+	{
+		//Mat temp = edge.rowRange(max(y1, 0), min(y2 + 4, 479)).colRange(x1, x2);
+		Mat temp = edge.col(i); //取第i列
+        // std::cout << "temp: " <<temp << std::endl;
+		cv::findNonZero(temp, idx);
+        std::vector<float> point_b = pic2cam(x1 + i, 240);  //转为相机坐标系
+		std::vector<float> point_H = pic2cam(320, 240);
+        float alfa = atan((point_b[0] - point_H[0]) / q.at<double>(2, 3));
+        if (idx.size() < 1)
+        {
+            Z.at<float>(0, i) = 0;
+		    Z.at<float>(1, i) = alfa;
+            continue;
+        }
+		int y_b = idx[idx.size() - 1].y + y1; //
+        y_b = int(y_b + y_b*0.03);
+		int y_h = 240;
+		point_b = pic2cam(x1 + i, y_b);  //转为相机坐标系
+		point_H = pic2cam(320, y_h);
+		Y_B = point_b[1];
+		Y_H = point_H[1];
+	
+        float H_c = 60; //摄像头离地高度，单位mm
+        float theta = 0; //摄像头与地面夹角，弧度
+		float d = (1/cos(theta)* cos(theta)) * q.at<double>(2, 3) * H_c / (Y_B - Y_H)- H_c*tan(theta);
+		alfa = atan((point_b[0] - point_H[0]) / q.at<double>(2, 3));
+        //cout << "d: " << d << endl;
+        if (d > 700)
+        {d = 0;}
+		Z.at<float>(0, i) = d/cos(alfa);
+		Z.at<float>(1, i) = alfa;
+	}
+
+    this->Z = Z.clone();
+}
+
+void Ranging::getInfo(Mat &imgL, Mat &imgR, Mat &detBoxes, Mat &info)
+{
+    Mat imgGrayL, imgGrayR;
+    cvtColor(imgL, imgGrayL, COLOR_BGR2GRAY);
+    cvtColor(imgR, imgGrayR, COLOR_BGR2GRAY);
+    Mat imgR_weight = imgR.clone();
+    Mat infoRow;
+    for (uchar i = 0; i < detBoxes.rows; i++)
+    {
+        int x1 = detBoxes.at<float>(i, 0);
+        int y1 = detBoxes.at<float>(i, 1);
+        int x2 = detBoxes.at<float>(i, 2);
+        int y2 = detBoxes.at<float>(i, 3);
+
+        float Conf = detBoxes.at<float>(i, 4);
+                
+        if (x1 > 600 || x2 < 50 || y1 < 5 || y2 > 475 || x1 < 2 || x2 > 590 || abs(x2 - x1) > 550) //当目标框偏左、偏右或者过大，略去该物体
+        {
+            continue;
+        }
+
+        // rectangle(imgR, Point(int(x1), int(y1)),
+        //           Point(int(x2), int(y2)), Scalar(0, 0, 255)); //绘制目标框
+       
+        int coordinates[4] = {x1, y1, x2, y2};
+        std::vector<int> disp_pixel = muban(imgGrayL, imgGrayR, coordinates); //模板匹配
+        float disp_pixel_x = disp_pixel[0] - x1; //ˮ计算水平视差
+        float disp_pixel_y = disp_pixel[1] - y1; //
+        disp_pixel_x = (int)(disp_pixel_x + disp_pixel_x * 0.12); //0.12为模板匹配产生的误差，为经验值，通过拟合得到
+        
+        //Mat disp_matrix = Mat(1, 1, CV_32F, Scalar(disp_pixel_x)), disp_pixel_xyz;
+        Mat disp_matrix = Mat(imgGrayL.rows, imgGrayL.cols, CV_32F, Scalar(disp_pixel_x)); //定义视差矩阵，所有值均为水平视差，方便转换为三维坐标，并具有水平距离信息
+        Mat threed_pixel_xyz, threedImage; 
+        reprojectImageTo3D(disp_matrix, threedImage, q, false); //2d->3d
+        threed_pixel_xyz = threedImage.mul(threedImage); //每一像素点求平方，
+        std::vector<Mat> channels;
+        split(threed_pixel_xyz.clone(), channels);
+        threed_pixel_xyz = channels[0] + channels[1] + channels[2]; //计算欧式距离
+        threed_pixel_xyz.forEach<float>([](float &value, const int *position) { value = sqrt(value); }); // 获得距离d
+       
+        int mid_pixel = int((x1 + x2) / 2);
+        std::vector<float> mid = pic2cam(imgGrayR.cols / 2, imgGrayR.rows); //计算角度，从像素坐标转为相机坐标
+        std::vector<float> loc_tar = pic2cam(mid_pixel, imgGrayR.rows);
+        float alfa = atan((loc_tar[0] - mid[0]) / q.at<double>(2, 3));
+
+
+        if (disp_pixel_x > 240)        // 距离太近，视差过大
+        {
+            char cm[15];
+            //sprintf(cm, "cannot match !");
+            // sprintf(cm, "%d , %.2f", cls,Conf);
+            putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
+            infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
+            infoRow.copyTo(info.row(i));
+            continue;
+        }
+        else
+        {
+            float median = threed_pixel_xyz.at<float>((int)(y1 + y2) / 2, (int)(x1 + x2) / 2);
+
+            std::vector<float> ltPoint = pic2cam(x1, y1);
+            std::vector<float> rbPoint = pic2cam(x2, y2);
+            float xx1 = ltPoint[0], yy1 = ltPoint[1], xx2 = rbPoint[0], yy2 = rbPoint[1]; //计算宽高
+            float f = q.at<double>(2, 3);
+            float f1 = sqrt(xx1 * xx1 + yy1 * yy1 + f * f); //推导得出
+            //float w1 = median * sqrt((xx1 - xx2) * (xx1 - xx2) / 4) / f1;
+            float h1 = median * sqrt((yy1 - yy2) * (yy1 - yy2) / 4) / f1;
+            float f2 = sqrt(xx2 * xx2 + yy2 * yy2 + f * f);
+            //float w2 = median * sqrt((xx2 - xx1) * (xx2 - xx1) / 4) / f2;
+            float h2 = median * sqrt((yy2 - yy1) * (yy2 - yy1) / 4) / f2;
+            float w1 = sqrt(pow((threedImage.at<cv::Vec3f>(y2, x1)[0] - threedImage.at<cv::Vec3f>(y2, x2)[0]), 2) +
+                            pow((threedImage.at<cv::Vec3f>(y2, x1)[1] - threedImage.at<cv::Vec3f>(y2, x2)[1]), 2) +
+                            pow((threedImage.at<cv::Vec3f>(y2, x1)[2] - threedImage.at<cv::Vec3f>(y2, x2)[2]), 2));
+
+            w1 = w1 / 10;
+            h1 = (h1 + h2) / 10;
+            median /= 10;
+            if (median > 120) //过远测距误差较大
+            {
+                //char tf[9];
+                //sprintf(tf, "Too far!");
+                char cm[15];
+                //sprintf(cm, "cannot match !");
+                // sprintf(cm, "%d , %.2f", cls,Conf);
+                putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
+                infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
+                infoRow.copyTo(info.row(i));
+                continue;
+            }
+            // <20><>ͼ<EFBFBD><CDBC><EFBFBD>ϻ<EFBFBD><CFBB><EFBFBD><EFBFBD><EFBFBD>Ϣ
+            // char dc[50], wh[50];
+            // std::string cname = className[cls + 1];
+            // sprintf(dc, "dis:%.2fcm  %d", median, cls);
+            // sprintf(wh, "W: %.2fcm H: %.2fcm alfa: %2f", w1, h1, alfa);
+            // putText(imgR, dc, Point(x1, y2), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 2);
+            // putText(imgR, wh, Point(x1, y1), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 1.5);
+
+		    
+            //返回数据
+            infoRow = (Mat_<float>(1, 4) << median, w1, h1, alfa);
+            infoRow.copyTo(info.row(i));
+        };
+    }
+        //    cv::imshow("kk",imgR);
+        //     cv::waitKey(1);
+
+}
+
+Ranging::Ranging(int index, int imgw, int imgh) : //初始化
+    mapX1(imgh, imgw, CV_64F),   //初始化矩阵  ，用于计算无畸变和修正转换映射。                                
+    mapX2(imgh, imgw, CV_64F),                                     
+    mapY1(imgh, imgw, CV_64F),
+    mapY2(imgh, imgw, CV_64F),
+    q(4, 4, CV_64F),
+    imgw(imgw),
+    imgh(imgh)
+{
+    // Z = Mat::zeros(2, 1, CV_32FC1);
+        vcapture = cv::VideoCapture(index);
+        //vcapture.set(CAP_PROP_FOURCC, CAP_OPENCV_MJPEG);
+        vcapture.set(cv::CAP_PROP_FPS, 30);
+        vcapture.set(cv::CAP_PROP_FRAME_WIDTH, imgw * 2);
+        vcapture.set(cv::CAP_PROP_FRAME_HEIGHT, imgh);
+	    vcapture.set(cv::CAP_PROP_BUFFERSIZE, 1);
+                        
+
+        auto imgSize = Size(imgw, imgh);
+        Mat r1(3, 3, CV_64F), r2(3, 3, CV_64F), p1(3, 4, CV_64F), p2(3, 4, CV_64F);
+
+        stereoRectify(cam_matrix_left.t(), distortion_l, cam_matrix_right.t(), distortion_r,
+                      imgSize, rotate.t(), trans, r1, r2, p1, p2, q);//立体校正
+
+        initUndistortRectifyMap(cam_matrix_left.t(), distortion_l, r1, p1, imgSize, CV_32F, mapX1, mapX2);//计算无畸变和修正转换映射
+        initUndistortRectifyMap(cam_matrix_right.t(), distortion_r, r2, p2, imgSize, CV_32F, mapY1, mapY2);//计算无畸变和修正转换映射
+		
+        // RKNN_Create(&hdx, modelPath); // 初始化检测模型
+	std::cout<< " ******************* CAMERA  initialization ********************" << std::endl;
+    
+}
+
+
+
+
+std::vector<Mat> Ranging::get_range() 
+{
+    
+	double rang_old, rang_now;
+	// rang_old = ros::Time::now().toSec(); //测试运行时间
+    Mat frame, lframe, rframe;
+    // vcapture.read(frame); //获取视频帧
+    vcapture >> frame;
+    
+    // cv::imshow("frame",frame);
+    
+    if (!frame.empty())
+    {
+        int64 t = getTickCount();
+        Mat lframe(frame.colRange(0, imgw).clone()); //拷贝左图
+        Mat rframe(frame.colRange(imgw, imgw * 2).clone()); //拷贝右图
+   
+        rectifyImage(lframe, rframe); //
+
+        cv::Mat Rframe = rframe.clone();
+
+        detect_result_group = yolov5s.outputParse(Rframe);
+
+        if (detect_result_group.count<=0)
+        {
+            std::cout<<"detect nothing"<<std::endl;
+        }
+
+        // detction box transfor to our format
+        Mat detBoxes(detect_result_group.count, 5, CV_32F); //定义矩阵，存储目标检测内容，存储格式(x,y,x,y,conf,cls)
+        char text[256];
+        int count = 0;
+
+        for (int i = 0; i < detect_result_group.count; i++) //存储目标检测内容 (x,y,x,y,conf,cls)
+        {
+            detect_result_t *det_result = &(detect_result_group.results[i]);
+
+            // if(strcmp(det_result->name, "vase") == 0 || strcmp(det_result->name, "suitcase") == 0)
+            if(strcmp(det_result->name, "vase") == 0|| strcmp(det_result->name, "suitcase") == 0||
+                // strcmp(det_result->name, "mouse") == 0|| strcmp(det_result->name, "keyboard") == 0||
+            //     strcmp(det_result->name, "mouse") == 0|| strcmp(det_result->name, "bowl") == 0||
+                strcmp(det_result->name, "cup") == 0|| strcmp(det_result->name, "bottle")==0)
+            {
+                count++;
+                sprintf(text, "%s %.1f%%", det_result->name, det_result->prop * 100);
+                // printf("%s @ (%d %d %d %d) %f\n", det_result->name, det_result->box.left, det_result->box.top,
+                // det_result->box.right, det_result->box.bottom, det_result->prop);
+                
+                int xmin = det_result->box.left;
+                int ymin = det_result->box.top;
+                int xmax = det_result->box.right;
+                int ymax = det_result->box.bottom;
+                
+                rectangle(Rframe, cv::Point(xmin, ymin), cv::Point(xmax, ymax), cv::Scalar(256, 0, 0, 256), 3);
+                putText(Rframe, text, cv::Point(xmin, ymin + 12), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255, 255, 255));
+                
+                // (x,y) (x,y) conf 
+                detBoxes.at<float>(count-1, 0) = xmin;
+                detBoxes.at<float>(count-1, 1) = ymin;
+                detBoxes.at<float>(count-1, 2) = xmax;
+                detBoxes.at<float>(count-1, 3) = ymax;
+                detBoxes.at<float>(count-1, 4) = det_result->prop;    
+                // detBoxes.at<float>(i, 0) = xmin;
+                // detBoxes.at<float>(i, 1) = ymin;
+                // detBoxes.at<float>(i, 2) = xmax;
+                // detBoxes.at<float>(i, 3) = ymax;
+                // detBoxes.at<float>(i, 4) = det_result->prop;   
+            }   
+            // 实验测试，过滤过大的误检框*/ 
+            //     float ratio = (xmax - xmin) * (ymax - ymin) / 308480.;
+            //     if (ratio > 0.7)
+            //     {
+            //         detBoxes.at<float>(i, 5) = -1;
+            //         continue;
+            //     }
+        }
+        Mat finalBoxes(detBoxes, Range(0, count), Range::all());
+        Mat info(count, 4, CV_32F); // 存储测距信息，存储格式：距离d，宽w，高h，角度α
+        // Mat info(detect_result_group.count, 4, CV_32F); // 存储测距信息，存储格式：距离d，宽w，高h，角度α
+        if (count)
+        // if (detect_result_group.count)
+        {
+            // getInfo(lframe, rframe, finalBoxes, info);
+            getInfo(lframe, rframe, finalBoxes, info);
+
+            for(int i=0; i<info.rows;i++)
+            {
+                float median = info.at<float>(i, 0);
+                // float w1 = info.at<float>(i, 1);
+                // float h1 = info.at<float>(i, 2);
+                // float alfa = info.at<float>(i, 3);
+
+                int x1 = finalBoxes.at<float>(i,0);
+                int y1 = finalBoxes.at<float>(i,1);
+                int x2 = finalBoxes.at<float>(i,2);
+                int y2 = finalBoxes.at<float>(i,3);
+
+                // int x1 = detBoxes.at<float>(i,0);
+                // int y1 = detBoxes.at<float>(i,1);
+                // int x2 = detBoxes.at<float>(i,2);
+                // int y2 = detBoxes.at<float>(i,3);
+                char dc[50], wh[50];
+                sprintf(dc, "dis:%.2fcm ", median);
+                // sprintf(wh, "W: %.2fcm H: %.2fcm ", w1, h1);
+                putText(Rframe, dc, Point(x1, y2), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255, 255, 255));
+                // putText(Rframe, wh, Point(x1, y1), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255, 255, 255));
+            }
+        }
+        t = getTickCount() - t;
+        char fps[50];
+        sprintf(fps, "fps: %d", int(1 / (t / getTickFrequency())));
+        putText(Rframe, fps, Point(20, 20), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 1.5);
+           
+        cv::imshow("k",Rframe);
+        cv::waitKey(1);
+        return std::vector<Mat>{rframe, detBoxes, info};
+    }
+    return std::vector<Mat>{empty};
+}
+
+

obj_dec/src/rknn_yolov5_demo/src/sub_dis.cc

@@ -0,0 +1,44 @@
+#include "ros/ros.h"
+#include "rknn_yolov5_demo/dis_info.h"
+#include "rknn_yolov5_demo/dis_info_array.h"
+
+void doMsg(const rknn_yolov5_demo::dis_info_array::ConstPtr& ceju_msg){
+ 
+    for (const auto& obstacle_info : ceju_msg->dis)
+    {
+        
+        float distance = obstacle_info.distance;
+        float width = obstacle_info.width;
+        float height = obstacle_info.height;
+        ROS_INFO("distance: %.2f ", distance);
+
+        // // 执行避障逻辑
+        // if (distance < obstacle_distance_threshold)
+        // {
+        //     ROS_INFO("Obstacle detected at distance: %.2f meters. Avoiding obstacle.", distance);
+
+        //     // 在这里执行避障动作，例如停止机器人
+        //     geometry_msgs::Twist cmd_vel;
+        //     cmd_vel.linear.x = 0.0;
+        //     cmd_vel.angular.z = 0.0;
+        //     cmd_vel_pub.publish(cmd_vel);
+
+        //     // 这里可以添加更复杂的避障逻辑，例如避开障碍物或调整方向
+        // }
+        // else
+        // {
+        //     ROS_INFO("No obstacle detected at distance: %.2f meters. Continuing.", distance);
+        // }
+    }
+}
+int main(int argc, char  *argv[])
+{
+    setlocale(LC_ALL,"");
+    ros::init(argc,argv,"sub_dis");
+    ros::NodeHandle nh;
+    ros::Subscriber sub = nh.subscribe<rknn_yolov5_demo::dis_info_array>("ceju_info",10,doMsg);
+  
+    ros::spin();//循环读取接收的数据，并调用回调函数处理
+
+    return 0;
+}

obj_dec/src/runtime/RK3588/Linux/librknn_api/aarch64/librknn_api.so

@@ -0,0 +1 @@
+librknnrt.so

obj_dec/src/runtime/RK3588/Linux/librknn_api/include/rknn_api.h

@@ -0,0 +1,720 @@
+/****************************************************************************
+*
+*    Copyright (c) 2017 - 2022 by Rockchip Corp.  All rights reserved.
+*
+*    The material in this file is confidential and contains trade secrets
+*    of Rockchip Corporation. This is proprietary information owned by
+*    Rockchip Corporation. No part of this work may be disclosed,
+*    reproduced, copied, transmitted, or used in any way for any purpose,
+*    without the express written permission of Rockchip Corporation.
+*
+*****************************************************************************/
+
+
+#ifndef _RKNN_API_H
+#define _RKNN_API_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+/*
+    Definition of extended flag for rknn_init.
+*/
+/* set high priority context. */
+#define RKNN_FLAG_PRIOR_HIGH                    0x00000000
+
+/* set medium priority context */
+#define RKNN_FLAG_PRIOR_MEDIUM                  0x00000001
+
+/* set low priority context. */
+#define RKNN_FLAG_PRIOR_LOW                     0x00000002
+
+/* asynchronous mode.
+   when enable, rknn_outputs_get will not block for too long because it directly retrieves the result of
+   the previous frame which can increase the frame rate on single-threaded mode, but at the cost of
+   rknn_outputs_get not retrieves the result of the current frame.
+   in multi-threaded mode you do not need to turn this mode on. */
+#define RKNN_FLAG_ASYNC_MASK                    0x00000004
+
+/* collect performance mode.
+   when enable, you can get detailed performance reports via rknn_query(ctx, RKNN_QUERY_PERF_DETAIL, ...),
+   but it will reduce the frame rate. */
+#define RKNN_FLAG_COLLECT_PERF_MASK             0x00000008
+
+/* allocate all memory in outside, includes weight/internal/inputs/outputs */
+#define RKNN_FLAG_MEM_ALLOC_OUTSIDE             0x00000010
+
+/* weight sharing with the same network structure */
+#define RKNN_FLAG_SHARE_WEIGHT_MEM              0x00000020
+
+/* send fence fd from outside */
+#define RKNN_FLAG_FENCE_IN_OUTSIDE              0x00000040
+
+/* get fence fd from inside */
+#define RKNN_FLAG_FENCE_OUT_OUTSIDE             0x00000080
+
+/* dummy init flag: could only get total_weight_size and total_internal_size by rknn_query*/
+#define RKNN_FLAG_COLLECT_MODEL_INFO_ONLY       0x00000100
+
+/* set GPU as the preferred execution backend When the operator is not supported by the NPU */
+#define RKNN_FLAG_EXECUTE_FALLBACK_PRIOR_DEVICE_GPU 0x00000400
+
+/* allocate internal memory in outside */
+#define RKNN_FLAG_INTERNAL_ALLOC_OUTSIDE        0x00000200
+
+/*
+    Error code returned by the RKNN API.
+*/
+#define RKNN_SUCC                               0       /* execute succeed. */
+#define RKNN_ERR_FAIL                           -1      /* execute failed. */
+#define RKNN_ERR_TIMEOUT                        -2      /* execute timeout. */
+#define RKNN_ERR_DEVICE_UNAVAILABLE             -3      /* device is unavailable. */
+#define RKNN_ERR_MALLOC_FAIL                    -4      /* memory malloc fail. */
+#define RKNN_ERR_PARAM_INVALID                  -5      /* parameter is invalid. */
+#define RKNN_ERR_MODEL_INVALID                  -6      /* model is invalid. */
+#define RKNN_ERR_CTX_INVALID                    -7      /* context is invalid. */
+#define RKNN_ERR_INPUT_INVALID                  -8      /* input is invalid. */
+#define RKNN_ERR_OUTPUT_INVALID                 -9      /* output is invalid. */
+#define RKNN_ERR_DEVICE_UNMATCH                 -10     /* the device is unmatch, please update rknn sdk
+                                                           and npu driver/firmware. */
+#define RKNN_ERR_INCOMPATILE_PRE_COMPILE_MODEL  -11     /* This RKNN model use pre_compile mode, but not compatible with current driver. */
+#define RKNN_ERR_INCOMPATILE_OPTIMIZATION_LEVEL_VERSION  -12     /* This RKNN model set optimization level, but not compatible with current driver. */
+#define RKNN_ERR_TARGET_PLATFORM_UNMATCH        -13     /* This RKNN model set target platform, but not compatible with current platform. */
+
+/*
+    Definition for tensor
+*/
+#define RKNN_MAX_DIMS                           16      /* maximum dimension of tensor. */
+#define RKNN_MAX_NUM_CHANNEL                    15      /* maximum channel number of input tensor. */
+#define RKNN_MAX_NAME_LEN                       256     /* maximum name lenth of tensor. */
+#define RKNN_MAX_DYNAMIC_SHAPE_NUM              512     /* maximum number of dynamic shape for each input. */
+
+#ifdef __arm__
+typedef uint32_t rknn_context;
+#else
+typedef uint64_t rknn_context;
+#endif
+
+
+/*
+    The query command for rknn_query
+*/
+typedef enum _rknn_query_cmd {
+    RKNN_QUERY_IN_OUT_NUM = 0,                              /* query the number of input & output tensor. */
+    RKNN_QUERY_INPUT_ATTR = 1,                              /* query the attribute of input tensor. */
+    RKNN_QUERY_OUTPUT_ATTR = 2,                             /* query the attribute of output tensor. */
+    RKNN_QUERY_PERF_DETAIL = 3,                             /* query the detail performance, need set
+                                                               RKNN_FLAG_COLLECT_PERF_MASK when call rknn_init,
+                                                               this query needs to be valid after rknn_outputs_get. */
+    RKNN_QUERY_PERF_RUN = 4,                                /* query the time of run,
+                                                               this query needs to be valid after rknn_outputs_get. */
+    RKNN_QUERY_SDK_VERSION = 5,                             /* query the sdk & driver version */
+
+    RKNN_QUERY_MEM_SIZE = 6,                                /* query the weight & internal memory size */
+    RKNN_QUERY_CUSTOM_STRING = 7,                           /* query the custom string */
+
+    RKNN_QUERY_NATIVE_INPUT_ATTR = 8,                       /* query the attribute of native input tensor. */
+    RKNN_QUERY_NATIVE_OUTPUT_ATTR = 9,                      /* query the attribute of native output tensor. */
+
+    RKNN_QUERY_NATIVE_NC1HWC2_INPUT_ATTR = 8,               /* query the attribute of native input tensor. */
+    RKNN_QUERY_NATIVE_NC1HWC2_OUTPUT_ATTR = 9,              /* query the attribute of native output tensor. */
+
+    RKNN_QUERY_NATIVE_NHWC_INPUT_ATTR = 10,                 /* query the attribute of native input tensor. */
+    RKNN_QUERY_NATIVE_NHWC_OUTPUT_ATTR = 11,                /* query the attribute of native output tensor. */
+
+    RKNN_QUERY_DEVICE_MEM_INFO = 12,                        /* query the attribute of rknn memory information. */
+
+    RKNN_QUERY_INPUT_DYNAMIC_RANGE = 13,                    /* query the dynamic shape range of rknn input tensor. */
+    RKNN_QUERY_CURRENT_INPUT_ATTR = 14,                     /* query the current shape of rknn input tensor, only valid for dynamic rknn model*/
+    RKNN_QUERY_CURRENT_OUTPUT_ATTR = 15,                    /* query the current shape of rknn output tensor, only valid for dynamic rknn model*/
+
+    RKNN_QUERY_CURRENT_NATIVE_INPUT_ATTR = 16,              /* query the current native shape of rknn input tensor, only valid for dynamic rknn model*/
+    RKNN_QUERY_CURRENT_NATIVE_OUTPUT_ATTR = 17,             /* query the current native shape of rknn output tensor, only valid for dynamic rknn model*/
+
+
+    RKNN_QUERY_CMD_MAX
+} rknn_query_cmd;
+
+/*
+    the tensor data type.
+*/
+typedef enum _rknn_tensor_type {
+    RKNN_TENSOR_FLOAT32 = 0,                            /* data type is float32. */
+    RKNN_TENSOR_FLOAT16,                                /* data type is float16. */
+    RKNN_TENSOR_INT8,                                   /* data type is int8. */
+    RKNN_TENSOR_UINT8,                                  /* data type is uint8. */
+    RKNN_TENSOR_INT16,                                  /* data type is int16. */
+    RKNN_TENSOR_UINT16,                                 /* data type is uint16. */
+    RKNN_TENSOR_INT32,                                  /* data type is int32. */
+    RKNN_TENSOR_UINT32,                                 /* data type is uint32. */
+    RKNN_TENSOR_INT64,                                  /* data type is int64. */
+    RKNN_TENSOR_BOOL,
+
+    RKNN_TENSOR_TYPE_MAX
+} rknn_tensor_type;
+
+inline static const char* get_type_string(rknn_tensor_type type)
+{
+    switch(type) {
+    case RKNN_TENSOR_FLOAT32: return "FP32";
+    case RKNN_TENSOR_FLOAT16: return "FP16";
+    case RKNN_TENSOR_INT8: return "INT8";
+    case RKNN_TENSOR_UINT8: return "UINT8";
+    case RKNN_TENSOR_INT16: return "INT16";
+    case RKNN_TENSOR_UINT16: return "UINT16";
+    case RKNN_TENSOR_INT32: return "INT32";
+    case RKNN_TENSOR_UINT32: return "UINT32";
+    case RKNN_TENSOR_INT64: return "INT64";
+    case RKNN_TENSOR_BOOL: return "BOOL";
+    default: return "UNKNOW";
+    }
+}
+
+/*
+    the quantitative type.
+*/
+typedef enum _rknn_tensor_qnt_type {
+    RKNN_TENSOR_QNT_NONE = 0,                           /* none. */
+    RKNN_TENSOR_QNT_DFP,                                /* dynamic fixed point. */
+    RKNN_TENSOR_QNT_AFFINE_ASYMMETRIC,                  /* asymmetric affine. */
+
+    RKNN_TENSOR_QNT_MAX
+} rknn_tensor_qnt_type;
+
+inline static const char* get_qnt_type_string(rknn_tensor_qnt_type type)
+{
+    switch(type) {
+    case RKNN_TENSOR_QNT_NONE: return "NONE";
+    case RKNN_TENSOR_QNT_DFP: return "DFP";
+    case RKNN_TENSOR_QNT_AFFINE_ASYMMETRIC: return "AFFINE";
+    default: return "UNKNOW";
+    }
+}
+
+/*
+    the tensor data format.
+*/
+typedef enum _rknn_tensor_format {
+    RKNN_TENSOR_NCHW = 0,                               /* data format is NCHW. */
+    RKNN_TENSOR_NHWC,                                   /* data format is NHWC. */
+    RKNN_TENSOR_NC1HWC2,                                /* data format is NC1HWC2. */
+    RKNN_TENSOR_UNDEFINED,
+
+    RKNN_TENSOR_FORMAT_MAX
+} rknn_tensor_format;
+
+/*
+    the mode of running on target NPU core.
+*/
+typedef enum _rknn_core_mask {
+    RKNN_NPU_CORE_AUTO = 0,                                       /* default, run on NPU core randomly. */
+    RKNN_NPU_CORE_0 = 1,                                          /* run on NPU core 0. */
+    RKNN_NPU_CORE_1 = 2,                                          /* run on NPU core 1. */
+    RKNN_NPU_CORE_2 = 4,                                          /* run on NPU core 2. */
+    RKNN_NPU_CORE_0_1 = RKNN_NPU_CORE_0 | RKNN_NPU_CORE_1,        /* run on NPU core 0 and core 1. */
+    RKNN_NPU_CORE_0_1_2 = RKNN_NPU_CORE_0_1 | RKNN_NPU_CORE_2,    /* run on NPU core 0 and core 1 and core 2. */
+
+    RKNN_NPU_CORE_UNDEFINED,
+} rknn_core_mask;
+
+inline static const char* get_format_string(rknn_tensor_format fmt)
+{
+    switch(fmt) {
+    case RKNN_TENSOR_NCHW: return "NCHW";
+    case RKNN_TENSOR_NHWC: return "NHWC";
+    case RKNN_TENSOR_NC1HWC2: return "NC1HWC2";
+    case RKNN_TENSOR_UNDEFINED: return "UNDEFINED";
+    default: return "UNKNOW";
+    }
+}
+
+/*
+    the information for RKNN_QUERY_IN_OUT_NUM.
+*/
+typedef struct _rknn_input_output_num {
+    uint32_t n_input;                                   /* the number of input. */
+    uint32_t n_output;                                  /* the number of output. */
+} rknn_input_output_num;
+
+/*
+    the information for RKNN_QUERY_INPUT_ATTR / RKNN_QUERY_OUTPUT_ATTR.
+*/
+typedef struct _rknn_tensor_attr {
+    uint32_t index;                                     /* input parameter, the index of input/output tensor,
+                                                           need set before call rknn_query. */
+
+    uint32_t n_dims;                                    /* the number of dimensions. */
+    uint32_t dims[RKNN_MAX_DIMS];                       /* the dimensions array. */
+    char name[RKNN_MAX_NAME_LEN];                       /* the name of tensor. */
+
+    uint32_t n_elems;                                   /* the number of elements. */
+    uint32_t size;                                      /* the bytes size of tensor. */
+
+    rknn_tensor_format fmt;                             /* the data format of tensor. */
+    rknn_tensor_type type;                              /* the data type of tensor. */
+    rknn_tensor_qnt_type qnt_type;                      /* the quantitative type of tensor. */
+    int8_t fl;                                          /* fractional length for RKNN_TENSOR_QNT_DFP. */
+    int32_t zp;                                         /* zero point for RKNN_TENSOR_QNT_AFFINE_ASYMMETRIC. */
+    float scale;                                        /* scale for RKNN_TENSOR_QNT_AFFINE_ASYMMETRIC. */
+
+    uint32_t w_stride;                                  /* the stride of tensor along the width dimention of input,
+                                                           Note: it is read-only, 0 means equal to width. */
+    uint32_t size_with_stride;                          /* the bytes size of tensor with stride. */
+
+    uint8_t pass_through;                               /* pass through mode, for rknn_set_io_mem interface.
+                                                           if TRUE, the buf data is passed directly to the input node of the rknn model
+                                                                    without any conversion. the following variables do not need to be set.
+                                                           if FALSE, the buf data is converted into an input consistent with the model
+                                                                     according to the following type and fmt. so the following variables
+                                                                     need to be set.*/
+    uint32_t h_stride;                                  /* the stride along the height dimention of input,
+                                                           Note: it is write-only, if it was set to 0, h_stride = height. */
+} rknn_tensor_attr;
+
+typedef struct _rknn_input_range {
+    uint32_t index;                                                 /* input parameter, the index of input/output tensor,
+                                                                        need set before call rknn_query. */
+    uint32_t shape_number;                                          /* the number of shape. */
+    rknn_tensor_format fmt;                                         /* the data format of tensor. */
+    char name[RKNN_MAX_NAME_LEN];                                   /* the name of tensor. */
+    uint32_t dyn_range[RKNN_MAX_DYNAMIC_SHAPE_NUM][RKNN_MAX_DIMS];  /* the dynamic input dimensions range. */
+    uint32_t n_dims;                                                /* the number of dimensions. */
+
+} rknn_input_range;
+
+/*
+    the information for RKNN_QUERY_PERF_DETAIL.
+*/
+typedef struct _rknn_perf_detail {
+    char* perf_data;                                    /* the string pointer of perf detail. don't need free it by user. */
+    uint64_t data_len;                                  /* the string length. */
+} rknn_perf_detail;
+
+/*
+    the information for RKNN_QUERY_PERF_RUN.
+*/
+typedef struct _rknn_perf_run {
+    int64_t run_duration;                               /* real inference time (us) */
+} rknn_perf_run;
+
+/*
+    the information for RKNN_QUERY_SDK_VERSION.
+*/
+typedef struct _rknn_sdk_version {
+    char api_version[256];                              /* the version of rknn api. */
+    char drv_version[256];                              /* the version of rknn driver. */
+} rknn_sdk_version;
+
+/*
+    the information for RKNN_QUERY_MEM_SIZE.
+*/
+typedef struct _rknn_mem_size {
+    uint32_t total_weight_size;                         /* the weight memory size */
+    uint32_t total_internal_size;                       /* the internal memory size, exclude inputs/outputs */
+    uint64_t total_dma_allocated_size;                  /* total dma memory allocated size */
+    uint32_t total_sram_size;                           /* total system sram size reserved for rknn */
+    uint32_t free_sram_size;                            /* free system sram size reserved for rknn */
+    uint32_t reserved[10];                              /* reserved */
+} rknn_mem_size;
+
+/*
+    the information for RKNN_QUERY_CUSTOM_STRING.
+*/
+typedef struct _rknn_custom_string {
+    char string[1024];                                  /* the string of custom, lengths max to 1024 bytes */
+} rknn_custom_string;
+
+/*
+   The flags of rknn_tensor_mem.
+*/
+typedef enum _rknn_tensor_mem_flags {
+    RKNN_TENSOR_MEMORY_FLAGS_ALLOC_INSIDE = 1,           /*Used to mark in rknn_destroy_mem() whether it is necessary to release the "mem" pointer itself.
+                                                         If the flag RKNN_TENSOR_MEMORY_FLAGS_ALLOC_INSIDE is set, rknn_destroy_mem() will call free(mem).*/
+    RKNN_TENSOR_MEMORY_FLAGS_FROM_FD      = 2,           /*Used to mark in rknn_create_mem_from_fd() whether it is necessary to release the "mem" pointer itself.
+                                                         If the flag RKNN_TENSOR_MEMORY_FLAGS_FROM_FD is set, rknn_destroy_mem() will call free(mem).*/
+    RKNN_TENSOR_MEMORY_FLAGS_FROM_PHYS    = 3,           /*Used to mark in rknn_create_mem_from_phys() whether it is necessary to release the "mem" pointer itself.
+                                                         If the flag RKNN_TENSOR_MEMORY_FLAGS_FROM_PHYS is set, rknn_destroy_mem() will call free(mem).*/
+    RKNN_TENSOR_MEMORY_FLAGS_UNKNOWN
+} rknn_tensor_mem_flags;
+
+/*
+    the memory information of tensor.
+*/
+typedef struct _rknn_tensor_memory {
+    void*            virt_addr;                         /* the virtual address of tensor buffer. */
+    uint64_t         phys_addr;                         /* the physical address of tensor buffer. */
+    int32_t          fd;                                /* the fd of tensor buffer. */
+    int32_t          offset;                            /* indicates the offset of the memory. */
+    uint32_t         size;                              /* the size of tensor buffer. */
+    uint32_t         flags;                             /* the flags of tensor buffer, reserved */
+    void *           priv_data;                         /* the private data of tensor buffer. */
+} rknn_tensor_mem;
+
+/*
+    the input information for rknn_input_set.
+*/
+typedef struct _rknn_input {
+    uint32_t index;                                     /* the input index. */
+    void* buf;                                          /* the input buf for index. */
+    uint32_t size;                                      /* the size of input buf. */
+    uint8_t pass_through;                               /* pass through mode.
+                                                           if TRUE, the buf data is passed directly to the input node of the rknn model
+                                                                    without any conversion. the following variables do not need to be set.
+                                                           if FALSE, the buf data is converted into an input consistent with the model
+                                                                     according to the following type and fmt. so the following variables
+                                                                     need to be set.*/
+    rknn_tensor_type type;                              /* the data type of input buf. */
+    rknn_tensor_format fmt;                             /* the data format of input buf.
+                                                           currently the internal input format of NPU is NCHW by default.
+                                                           so entering NCHW data can avoid the format conversion in the driver. */
+} rknn_input;
+
+/*
+    the output information for rknn_outputs_get.
+*/
+typedef struct _rknn_output {
+    uint8_t want_float;                                 /* want transfer output data to float */
+    uint8_t is_prealloc;                                /* whether buf is pre-allocated.
+                                                           if TRUE, the following variables need to be set.
+                                                           if FALSE, the following variables do not need to be set. */
+    uint32_t index;                                     /* the output index. */
+    void* buf;                                          /* the output buf for index.
+                                                           when is_prealloc = FALSE and rknn_outputs_release called,
+                                                           this buf pointer will be free and don't use it anymore. */
+    uint32_t size;                                      /* the size of output buf. */
+} rknn_output;
+
+/*
+    the extend information for rknn_init.
+*/
+typedef struct _rknn_init_extend {
+    rknn_context ctx;                                    /* rknn context */
+    int32_t      real_model_offset;                      /* real rknn model file offset, only valid when init context with rknn file path */
+    uint32_t     real_model_size;                        /* real rknn model file size, only valid when init context with rknn file path */
+    uint8_t      reserved[120];                          /* reserved */
+} rknn_init_extend;
+
+/*
+    the extend information for rknn_run.
+*/
+typedef struct _rknn_run_extend {
+    uint64_t frame_id;                                  /* output parameter, indicate current frame id of run. */
+    int32_t non_block;                                  /* block flag of run, 0 is block else 1 is non block */
+    int32_t timeout_ms;                                 /* timeout for block mode, in milliseconds */
+    int32_t fence_fd;                                   /* fence fd from other unit */
+} rknn_run_extend;
+
+/*
+    the extend information for rknn_outputs_get.
+*/
+typedef struct _rknn_output_extend {
+    uint64_t frame_id;                                  /* output parameter, indicate the frame id of outputs, corresponds to
+                                                           struct rknn_run_extend.frame_id.*/
+} rknn_output_extend;
+
+
+/*  rknn_init
+
+    initial the context and load the rknn model.
+
+    input:
+        rknn_context* context       the pointer of context handle.
+        void* model                 if size > 0, pointer to the rknn model, if size = 0, filepath to the rknn model.
+        uint32_t size               the size of rknn model.
+        uint32_t flag               extend flag, see the define of RKNN_FLAG_XXX_XXX.
+        rknn_init_extend* extend    the extend information of init.
+    return:
+        int                         error code.
+*/
+int rknn_init(rknn_context* context, void* model, uint32_t size, uint32_t flag, rknn_init_extend* extend);
+
+/*  rknn_dup_context
+
+    initial the context and load the rknn model.
+
+    input:
+        rknn_context* context_in       the pointer of context in handle.
+        rknn_context* context_out      the pointer of context out handle.
+    return:
+        int                         error code.
+*/
+int rknn_dup_context(rknn_context* context_in, rknn_context* context_out);
+
+/*  rknn_destroy
+
+    unload the rknn model and destroy the context.
+
+    input:
+        rknn_context context        the handle of context.
+    return:
+        int                         error code.
+*/
+int rknn_destroy(rknn_context context);
+
+
+/*  rknn_query
+
+    query the information about model or others. see rknn_query_cmd.
+
+    input:
+        rknn_context context        the handle of context.
+        rknn_query_cmd cmd          the command of query.
+        void* info                  the buffer point of information.
+        uint32_t size               the size of information.
+    return:
+        int                         error code.
+*/
+int rknn_query(rknn_context context, rknn_query_cmd cmd, void* info, uint32_t size);
+
+
+/*  rknn_inputs_set
+
+    set inputs information by input index of rknn model.
+    inputs information see rknn_input.
+
+    input:
+        rknn_context context        the handle of context.
+        uint32_t n_inputs           the number of inputs.
+        rknn_input inputs[]         the arrays of inputs information, see rknn_input.
+    return:
+        int                         error code
+*/
+int rknn_inputs_set(rknn_context context, uint32_t n_inputs, rknn_input inputs[]);
+
+/*
+    rknn_set_batch_core_num
+
+    set rknn batch core_num.
+
+    input:
+        rknn_context context        the handle of context.
+        int core_num                the core number.
+    return:
+        int                         error code.
+
+*/
+int rknn_set_batch_core_num(rknn_context context, int core_num);
+
+/*  rknn_set_core_mask
+
+    set rknn core mask.(only supported on RK3588 now)
+
+    RKNN_NPU_CORE_AUTO: auto mode, default value
+    RKNN_NPU_CORE_0: core 0 mode
+    RKNN_NPU_CORE_1: core 1 mode
+    RKNN_NPU_CORE_2: core 2 mode
+    RKNN_NPU_CORE_0_1: combine core 0/1 mode
+    RKNN_NPU_CORE_0_1_2: combine core 0/1/2 mode
+
+    input:
+        rknn_context context        the handle of context.
+        rknn_core_mask core_mask    the core mask.
+    return:
+        int                         error code.
+*/
+int rknn_set_core_mask(rknn_context context, rknn_core_mask core_mask);
+
+/*  rknn_run
+
+    run the model to execute inference.
+
+    input:
+        rknn_context context        the handle of context.
+        rknn_run_extend* extend     the extend information of run.
+    return:
+        int                         error code.
+*/
+int rknn_run(rknn_context context, rknn_run_extend* extend);
+
+
+/*  rknn_wait
+
+    wait the model after execute inference.
+
+    input:
+        rknn_context context        the handle of context.
+        rknn_run_extend* extend     the extend information of run.
+    return:
+        int                         error code.
+*/
+int rknn_wait(rknn_context context, rknn_run_extend* extend);
+
+
+/*  rknn_outputs_get
+
+    wait the inference to finish and get the outputs.
+    this function will block until inference finish.
+    the results will set to outputs[].
+
+    input:
+        rknn_context context        the handle of context.
+        uint32_t n_outputs          the number of outputs.
+        rknn_output outputs[]       the arrays of output, see rknn_output.
+        rknn_output_extend*         the extend information of output.
+    return:
+        int                         error code.
+*/
+int rknn_outputs_get(rknn_context context, uint32_t n_outputs, rknn_output outputs[], rknn_output_extend* extend);
+
+
+/*  rknn_outputs_release
+
+    release the outputs that get by rknn_outputs_get.
+    after called, the rknn_output[x].buf get from rknn_outputs_get will
+    also be free when rknn_output[x].is_prealloc = FALSE.
+
+    input:
+        rknn_context context        the handle of context.
+        uint32_t n_ouputs           the number of outputs.
+        rknn_output outputs[]       the arrays of output.
+    return:
+        int                         error code
+*/
+int rknn_outputs_release(rknn_context context, uint32_t n_ouputs, rknn_output outputs[]);
+
+
+/* new api for zero copy */
+
+/*  rknn_create_mem_from_phys (memory allocated outside)
+
+    initialize tensor memory from physical address.
+
+    input:
+        rknn_context ctx            the handle of context.
+        uint64_t phys_addr          physical address.
+        void *virt_addr             virtual address.
+        uint32_t size               the size of tensor buffer.
+    return:
+        rknn_tensor_mem             the pointer of tensor memory information.
+*/
+rknn_tensor_mem* rknn_create_mem_from_phys(rknn_context ctx, uint64_t phys_addr, void *virt_addr, uint32_t size);
+
+
+/*  rknn_create_mem_from_fd (memory allocated outside)
+
+    initialize tensor memory from file description.
+
+    input:
+        rknn_context ctx            the handle of context.
+        int32_t fd                  file description.
+        void *virt_addr             virtual address.
+        uint32_t size               the size of tensor buffer.
+        int32_t offset              indicates the offset of the memory (virt_addr without offset).
+    return:
+        rknn_tensor_mem             the pointer of tensor memory information.
+*/
+rknn_tensor_mem* rknn_create_mem_from_fd(rknn_context ctx, int32_t fd, void *virt_addr, uint32_t size, int32_t offset);
+
+
+/*  rknn_create_mem_from_mb_blk (memory allocated outside)
+
+    create tensor memory from mb_blk.
+
+    input:
+        rknn_context ctx            the handle of context.
+        void *mb_blk                mb_blk allocate from system api.
+        int32_t offset              indicates the offset of the memory.
+    return:
+        rknn_tensor_mem             the pointer of tensor memory information.
+*/
+rknn_tensor_mem* rknn_create_mem_from_mb_blk(rknn_context ctx, void *mb_blk, int32_t offset);
+
+
+/*  rknn_create_mem (memory allocated inside)
+
+    create tensor memory.
+
+    input:
+        rknn_context ctx            the handle of context.
+        uint32_t size               the size of tensor buffer.
+    return:
+        rknn_tensor_mem             the pointer of tensor memory information.
+*/
+rknn_tensor_mem* rknn_create_mem(rknn_context ctx, uint32_t size);
+
+
+/*  rknn_destroy_mem (support allocate inside and outside)
+
+    destroy tensor memory.
+
+    input:
+        rknn_context ctx            the handle of context.
+        rknn_tensor_mem *mem        the pointer of tensor memory information.
+    return:
+        int                         error code
+*/
+int rknn_destroy_mem(rknn_context ctx, rknn_tensor_mem *mem);
+
+
+/*  rknn_set_weight_mem
+
+    set the weight memory.
+
+    input:
+        rknn_context ctx            the handle of context.
+        rknn_tensor_mem *mem        the array of tensor memory information
+    return:
+        int                         error code.
+*/
+int rknn_set_weight_mem(rknn_context ctx, rknn_tensor_mem *mem);
+
+
+/*  rknn_set_internal_mem
+
+    set the internal memory.
+
+    input:
+        rknn_context ctx            the handle of context.
+        rknn_tensor_mem *mem        the array of tensor memory information
+    return:
+        int                         error code.
+*/
+int rknn_set_internal_mem(rknn_context ctx, rknn_tensor_mem *mem);
+
+
+/*  rknn_set_io_mem
+
+    set the input and output tensors buffer.
+
+    input:
+        rknn_context ctx            the handle of context.
+        rknn_tensor_mem *mem        the array of tensor memory information.
+        rknn_tensor_attr *attr      the attribute of input or output tensor buffer.
+    return:
+        int                         error code.
+*/
+int rknn_set_io_mem(rknn_context ctx, rknn_tensor_mem *mem, rknn_tensor_attr *attr);
+
+/*  rknn_set_input_shape(deprecated)
+
+    set the input tensor shape (only valid for dynamic shape rknn model).
+
+    input:
+        rknn_context ctx            the handle of context.
+        rknn_tensor_attr *attr      the attribute of input or output tensor buffer.
+    return:
+        int                         error code.
+*/
+int rknn_set_input_shape(rknn_context ctx, rknn_tensor_attr* attr);
+
+/*  rknn_set_input_shapes
+
+    set all the input tensor shapes. graph will run under current set of input shapes after rknn_set_input_shapes.(only valid for dynamic shape rknn model).
+
+    input:
+        rknn_context ctx            the handle of context.
+        uint32_t n_inputs           the number of inputs.
+        rknn_tensor_attr attr[]     the attribute array of all input tensors.
+    return:
+        int                         error code.
+*/
+int rknn_set_input_shapes(rknn_context ctx, uint32_t n_inputs, rknn_tensor_attr attr[]);
+
+#ifdef __cplusplus
+} //extern "C"
+#endif
+
+#endif  //_RKNN_API_H

obj_dec/src/runtime/RK3588/Linux/librknn_api/include/rknn_matmul_api.h

@@ -0,0 +1,261 @@
+/****************************************************************************
+ *
+ *    Copyright (c) 2017 - 2018 by Rockchip Corp.  All rights reserved.
+ *
+ *    The material in this file is confidential and contains trade secrets
+ *    of Rockchip Corporation. This is proprietary information owned by
+ *    Rockchip Corporation. No part of this work may be disclosed,
+ *    reproduced, copied, transmitted, or used in any way for any purpose,
+ *    without the express written permission of Rockchip Corporation.
+ *
+ *****************************************************************************/
+
+#ifndef _RKNN_MATMUL_API_H
+#define _RKNN_MATMUL_API_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "rknn_api.h"
+
+typedef rknn_context rknn_matmul_ctx;
+
+typedef struct _rknn_matmul_tensor_attr
+{
+  char name[RKNN_MAX_NAME_LEN];
+
+  // indicate A(M, K) or B(K, N) or C(M, N)
+  uint32_t n_dims;
+  uint32_t dims[RKNN_MAX_DIMS];
+
+  // matmul tensor size
+  uint32_t size;
+
+  // matmul tensor data type
+  // int8 : A, B
+  // int32: C
+  rknn_tensor_type type;
+} rknn_matmul_tensor_attr;
+
+typedef struct _rknn_matmul_io_attr
+{
+  // indicate A(M, K) or B(K, N) or C(M, N)
+  rknn_matmul_tensor_attr A;
+  rknn_matmul_tensor_attr B;
+  rknn_matmul_tensor_attr C;
+} rknn_matmul_io_attr;
+
+/*
+  matmul information struct
+ */
+typedef struct rknn_matmul_info_t
+{
+  int32_t M;
+  int32_t K; // limit: rk356x: int8 type must be aligned with 32byte, float16 type must be aligned with 16byte;
+             // rk3588: int8 type must be aligned with 32byte, float16 type must be aligned with 32byte;
+  int32_t N; // limit: rk356x: int8 type must be aligned with 16byte, float16 type must be aligned with 8byte;
+             // rk3588: int8 type must be aligned with 32byte, float16 type must be aligned with 16byte;
+
+  // matmul data type
+  // int8: int8(A) x int8(B) -> int32(C)
+  // float16: float16(A) x float16(B) -> float32(C)
+  rknn_tensor_type type;
+
+  // matmul native layout for B
+  // 0: normal layout
+  // 1: native layout
+  int32_t native_layout;
+
+  // matmul perf layout for A and C
+  // 0: normal layout
+  // 1: perf layout
+  int32_t perf_layout;
+} rknn_matmul_info;
+
+/*  rknn_matmul_create
+
+    params:
+        rknn_matmul_ctx *ctx           the handle of context.
+        rknn_matmul_info *info         the matmal information.
+        rknn_matmul_io_attr *io_attr   inputs/output attribute
+    return:
+        int                         error code
+*/
+int rknn_matmul_create(rknn_matmul_ctx* ctx, rknn_matmul_info* info, rknn_matmul_io_attr* io_attr);
+
+/* rknn_matmul_set_io_mem
+
+    params:
+        rknn_matmul_ctx ctx            the handle of context.
+        rknn_tensor_mem *mem           the pointer of tensor memory information.
+        rknn_matmul_tensor_attr *attr  the attribute of input or output tensor buffer.
+    return:
+        int                         error code.
+
+    formula:
+      C = A * B,
+
+    limit:
+      K <= 4096
+      K limit: rk356x: int8 type must be aligned with 32byte, float16 type must be aligned with 16byte;
+               rk3588: int8 type must be aligned with 32byte, float16 type must be aligned with 32byte;
+      N limit: rk356x: int8 type must be aligned with 16byte, float16 type must be aligned with 8byte;
+               rk3588: int8 type must be aligned with 32byte, float16 type must be aligned with 16byte;
+
+    A shape: M x K
+      normal layout: (M, K)
+              [M1K1, M1K2, ..., M1Kk,
+               M2K1, M2K2, ..., M2Kk,
+               ...
+               MmK1, MmK2, ..., MmKk]
+      for rk356x：
+      int8:
+      perf layout: (K / 8, M, 8)
+              [K1M1, K2M1,  ..., K8M1,
+               K9M2, K10M2, ..., K16M2,
+               ...
+               K(k-7)Mm, K(k-6)Mm, ..., KkMm]
+      float16:
+      perf layout: (K / 4, M, 4)
+              [K1M1, K2M1,  ..., K4M1,
+               K9M2, K10M2, ..., K8M2,
+               ...
+               K(k-3)Mm, K(k-2)Mm, ..., KkMm]
+      for rk3588：
+      int8:
+      perf layout: (K / 16, M, 16)
+              [K1M1, K2M1,  ..., K16M1,
+               K9M2, K10M2, ..., K32M2,
+               ...
+               K(k-15)Mm, K(k-14)Mm, ..., KkMm]
+      float16:
+      perf layout: (K / 8, M, 8)
+              [K1M1, K2M1,  ..., K8M1,
+               K9M2, K10M2, ..., K16M2,
+               ...
+               K(k-7)Mm, K(k-6)Mm, ..., KkMm]
+    B shape: K x N
+      normal layout: (K, N)
+              [K1N1, K1N2, ..., K1Nn,
+               K2N1, K2N2, ..., K2Nn,
+               ...
+               KkN1, KkN2, ..., KkNn]
+      for rk356x：
+      int8:
+      native layout: (N / 16, K / 32, 16, 32)
+              [K1N1,  K2N1,  ..., K32N1,
+               K1N2,  K2N2,  ..., K32N2,
+               ...
+               K1N16, K2N16, ..., K32N16,
+               K33N1, K34N1, ..., K64N1,
+               K33N2, K34N2, ..., K64N2,
+               ...
+               K(k-31)N16, K(k-30)N16, ..., KkN16,
+               K1N17, K2N17, ..., K32N17,
+               K1N18, K2N18, ..., K32N18,
+               ...
+               K(k-31)Nn, K(k-30)Nn, ..., KkNn]
+      float16:
+      native layout: (N / 8, K / 16, 8, 16)
+              [K1N1,  K2N1,  ..., K16N1,
+               K1N2,  K2N2,  ..., K16N2,
+               ...
+               K1N8,  K2N8,  ..., K16N8,
+               K17N1, K18N1, ..., K32N1,
+               K17N2, K18N2, ..., K32N2,
+               ...
+               K(k-15)N8, K(k-30)N8, ..., KkN8,
+               K1N9,  K2N9,  ..., K16N9,
+               K1N10, K2N10, ..., K16N10,
+               ...
+               K(k-15)Nn, K(k-14)Nn, ..., KkNn]
+      for rk3588：
+      int8:
+      native layout: (N / 32, K / 32, 32, 32)
+              [K1N1,  K2N1,  ..., K32N1,
+               K1N2,  K2N2,  ..., K32N2,
+               ...
+               K1N32, K2N32, ..., K32N32,
+               K33N1, K34N1, ..., K64N1,
+               K33N2, K34N2, ..., K64N2,
+               ...
+               K(k-31)N32, K(k-30)N32, ..., KkN32,
+               K1N33, K2N33, ..., K32N33,
+               K1N34, K2N34, ..., K32N34,
+               ...
+               K(k-31)Nn, K(k-30)Nn, ..., KkNn]
+      float16:
+      native layout: (N / 16, K / 32, 16, 32)
+              [K1N1,  K2N1,  ..., K32N1,
+               K1N2,  K2N2,  ..., K32N2,
+               ...
+               K1N16, K2N16, ..., K32N16,
+               K33N1, K34N1, ..., K64N1,
+               K33N2, K34N2, ..., K64N2,
+               ...
+               K(k-31)N16, K(k-30)N16, ..., KkN16,
+               K1N17, K2N17, ..., K32N17,
+               K1N18, K2N18, ..., K32N18,
+               ...
+               K(k-31)Nn, K(k-30)Nn, ..., KkNn]
+    C shape: M x N
+      normal layout: (M, N)
+              [M1N1, M1N2, ..., M1Nn,
+               M2N1, M2N2, ..., M2Nn,
+               ...
+               MmN1, MmN2, ..., MmNn]
+      perf layout: (N / 4, M, 4)
+              [N1M1, N2M1, ..., N4M1,
+               N5M2, N6M2, ..., N8M2,
+               ...
+               N(n-3)Mm, N(n-2)Mm, ..., NnMm]
+ */
+int rknn_matmul_set_io_mem(rknn_matmul_ctx ctx, rknn_tensor_mem* mem, rknn_matmul_tensor_attr* attr);
+
+/*  rknn_matmul_set_core_mask
+
+    set rknn core mask.(only support rk3588 in current)
+
+    RKNN_NPU_CORE_AUTO: auto mode, default value
+    RKNN_NPU_CORE_0: core 0 mode
+    RKNN_NPU_CORE_1: core 1 mode
+    RKNN_NPU_CORE_2: core 2 mode
+    RKNN_NPU_CORE_0_1: combine core 0/1 mode
+    RKNN_NPU_CORE_0_1_2: combine core 0/1/2 mode
+
+    input:
+        rknn_matmul_ctx context     the handle of context.
+        rknn_core_mask core_mask    the core mask.
+    return:
+        int                         error code.
+*/
+int rknn_matmul_set_core_mask(rknn_matmul_ctx context, rknn_core_mask core_mask);
+
+/*  rknn_matmul_run
+
+    run the matmul in blocking mode
+
+    params:
+        rknn_matmul_ctx ctx         the handle of context.
+    return:
+        int                         error code.
+ */
+int rknn_matmul_run(rknn_matmul_ctx ctx);
+
+/*  rknn_matmul_destroy
+
+    destroy the matmul context
+
+    params:
+        rknn_matmul_ctx ctx         the handle of context.
+    return:
+        int                         error code.
+ */
+int rknn_matmul_destroy(rknn_matmul_ctx ctx);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+
+#endif // _RKNN_MATMUL_API_H

obj_dec/src/runtime/RK3588/Linux/rknn_server/aarch64/usr/bin/restart_rknn.sh

@@ -0,0 +1,5 @@
+#!/bin/sh                                                                                                                                                     
+ 
+killall start_rknn.sh > /dev/null 2>&1
+killall rknn_server > /dev/null 2>&1
+start_rknn.sh &

obj_dec/src/runtime/RK3588/Linux/rknn_server/aarch64/usr/bin/start_rknn.sh

@@ -0,0 +1,7 @@
+#!/bin/sh
+
+while true
+do
+  sleep 1
+  rknn_server #>/dev/null 2>&1
+done

obj_dec/src/runtime/init.rknn_server.rc

@@ -0,0 +1,9 @@
+#on early-boot
+on init
+    start rknn_server
+
+
+service rknn_server /vendor/bin/rknn_server
+    class core
+    seclabel u:r:rknn_server:s0
+    disabled

pibot_ros/.gitignore

@@ -0,0 +1,9 @@
+.vscode
+*.pyc
+ros_ws/build
+ros_ws/devel
+ros_ws/install
+ros_package/*
+!ros_package/*.tar.gz
+cartographer
+ros_ws/src/CMakeLists.txt

pibot_ros/README.md

@@ -0,0 +1,31 @@
+# PIBOT ROS Workspace v2.0
+
+## install ros
+```shell
+cd ~/pibot_ros/
+./pibot_install_ros.sh
+source ~/.bashrc
+```
+
+## init environment
+for master
+```shell
+cd ~/pibot_ros/
+./pibot_init_env.sh # select board lidar 3dsensor
+source ~/.bashrc
+```
+
+for salve
+```shell
+cd ~/pibot_ros/
+./pibot_init_env.sh # select board lidar 3dsensor
+source ~/.bashrc
+```
+
+## run example
+```
+roslaunch pibot_bringup bringup.launch
+roslaunch pibot keyboard_teleop.launch
+```
+
+then you can control your robot

pibot_ros/pibot_add_upstart.sh

@@ -0,0 +1,15 @@
+#!/bin/bash
+if [ ! -z "$PIBOT_HOME" ]; then
+    PIBOT_HOME_DIR=$PIBOT_HOME
+else
+    PIBOT_HOME_DIR=~/pibot_ros
+fi
+
+sudo ln -sf $PIBOT_HOME_DIR/pibot_upstart/pibotenv /etc/pibotenv
+sudo ln -sf $PIBOT_HOME_DIR/pibot_upstart/pibot_start.sh /usr/bin/pibot_start
+sudo ln -sf $PIBOT_HOME_DIR/pibot_upstart/pibot_stop.sh /usr/bin/pibot_stop
+sudo ln -sf $PIBOT_HOME_DIR/pibot_upstart/pibot_restart.sh /usr/bin/pibot_restart
+sudo cp $PIBOT_HOME_DIR/pibot_upstart/pibot.service /etc/systemd/system/
+sudo systemctl daemon-reload
+sudo systemctl enable pibot
+sudo systemctl is-enabled pibot

pibot_ros/pibot_ap.sh

@@ -0,0 +1,46 @@
+#!/bin/bash
+
+# put this cmd to startup scritps
+# create pibot ap
+# if [ -f /home/pibot/.pibot_ap ]; then
+#     create_ap wlan0 eth0 pibot_ap pibot_ap&
+# else
+#     create_ap --fix-unmanaged
+#     ifconfig eth0 up
+#     ifconfig wlan0 up
+# fi
+
+function start() {
+	echo "start ap mode"
+	touch ~/.pibot_ap
+	# sudo nmcli c | awk -F' ' '{cmd="sudo nmcli c del "$1; if(NR>=2) system(cmd)}'
+	sudo nmcli --fields UUID con | awk '{print $1}' | while read line; do sudo nmcli con delete uuid $line; done
+	sudo systemctl restart create_ap
+	sudo systemctl enable create_ap
+}
+
+function stop() {
+	echo "stop ap mode"
+	if [ -f ~/.pibot_ap ]; then
+		rm ~/.pibot_ap
+	fi
+	sudo systemctl stop create_ap
+	sudo systemctl disable create_ap
+}
+
+case "$1" in
+	start )
+	echo  "****************"
+	start
+	echo  "****************"
+	;;
+	stop )
+	echo  "****************"
+	stop
+	echo  "****************"
+	;;
+	* )
+	echo  "****************"
+	echo  "$0 start/stop"
+	echo  "****************"
+esac

pibot_ros/pibot_clear_rosproc.sh

@@ -0,0 +1,4 @@
+#!/bin/bash
+# kill all ros&pibot process
+ps -aux | grep "/opt/ros" | grep -v "grep" | awk '{print $2}' | xargs kill -9
+ps -aux | grep "/pibot_ros/ros_ws" | grep -v "grep" | awk '{print $2}' | xargs kill -9

pibot_ros/pibot_create_swap.sh

@@ -0,0 +1,10 @@
+#!/bin/bash
+
+sudo mkdir /opt/image
+cd /opt/image/
+sudo touch swap
+sudo dd if=/dev/zero of=/opt/image/swap bs=1024 count=2048000
+sudo mkswap /opt/image/swap  
+sudo swapon /opt/image/swap 
+
+echo "add \"/opt/image/swap /swap swap defaults 0 0\" to \"/etc/fstab\" to make swap effecive"

pibot_ros/pibot_init_env.sh

@@ -0,0 +1,362 @@
+#!/bin/bash
+PIBOT_MODEL=
+PIBOT_BOARD=
+PIBOT_LIDAR=
+PIBOT_3DSENSOR=
+PIBOT_ADDRESS=
+
+while getopts "m:b:l:c:a:" opt; do
+  case $opt in
+    m)
+        PIBOT_MODEL=$OPTARG
+        echo $PIBOT_MODEL;;
+    b)
+        PIBOT_BOARD=$OPTARG
+        echo $PIBOT_BOARD;;
+    l)
+        PIBOT_LIDAR=$OPTARG
+        echo $PIBOT_LIDAR;;
+    c)
+        PIBOT_3DSENSOR=$OPTARG
+        echo $PIBOT_3DSENSOR;;
+    a)
+        PIBOT_ADDRESS=$OPTARG
+        echo $PIBOT_ADDRESS;;
+    \?)
+        echo "usage: "$0 "-m {MODLE} -b {BOARD_TYPE} -L {LIDAR} -c {CAMERA} -a {ROS_iP}"
+        echo -e "\033[1;32m   for example 1: $0 -m apollo -b stm32f1 -l rplidar -c none -a localhost\033[0m"
+        echo -e "\033[1;32m   for example 1: $0 -m hades -b stm32f4 -l rplidar -c none -a localhost\033[0m"
+        exit 0 ;;
+  esac
+done
+
+if [ ! -z "$PIBOT_HOME" ]; then
+    PIBOT_HOME_DIR=$PIBOT_HOME
+else
+    PIBOT_HOME_DIR=~/pibot_ros
+fi
+
+echo -e "\033[1;32mpibot home dir:$PIBOT_HOME_DIR"
+
+sudo ln -sf $PIBOT_HOME_DIR/pibot_init_env.sh /usr/bin/pibot_init_env
+sudo ln -sf $PIBOT_HOME_DIR/pibot_view_env.sh /usr/bin/pibot_view_env
+sudo ln -sf $PIBOT_HOME_DIR/pibot_install_ros.sh /usr/bin/pibot_install_ros
+
+if ! [ $PIBOT_ENV_INITIALIZED ]; then
+    echo "export PIBOT_ENV_INITIALIZED=1" >> ~/.bashrc
+    echo "source ~/.pibotrc" >> ~/.bashrc
+fi
+
+#rules
+echo -e "\033[1;32msetup pibot modules"
+echo " "
+sudo rm -rf /etc/udev/rules.d/pibot.rules
+sudo rm -rf /etc/udev/rules.d/rplidar.rules
+sudo rm -rf /etc/udev/rules.d/ydlidar.rules
+
+sudo cp $PIBOT_HOME_DIR/rules/98-pibot-usb.rules  /etc/udev/rules.d
+sudo cp $PIBOT_HOME_DIR/rules/56-orbbec-usb.rules  /etc/udev/rules.d
+echo " "
+echo "Restarting udev"
+echo ""
+
+sudo udevadm control --reload-rules
+sudo udevadm trigger
+#sudo service udev reload
+#sudo service udev restart
+
+code_name=$(lsb_release -sc)
+
+if [ "$code_name" = "trusty" ]; then
+    ROS_DISTRO="indigo"
+elif [ "$code_name" = "xenial" ]; then
+    ROS_DISTRO="kinetic"
+elif [ "$code_name" = "bionic" ] || [ "$code_name" = "stretch" ]; then
+    ROS_DISTRO="melodic"
+elif [ "$code_name" = "focal" ] || [ "$code_name" = "buster" ]; then
+    ROS_DISTRO="noetic"
+else
+    echo -e "\033[1;31m PIBOT not support "$code_name"\033[0m"
+    exit
+fi 
+
+
+content="#source ros
+if [ ! -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then 
+    echo \"please run cd $PIBOT_HOME_DIR && ./pibot_install_ros.sh to install ros sdk\"
+else
+    source /opt/ros/${ROS_DISTRO}/setup.bash
+fi
+"
+echo "${content}" > ~/.pibotrc
+
+#LOCAL_IP=`ifconfig eth0|grep "inet addr:"|awk -F":" '{print $2}'|awk '{print $1}'`
+#LOCAL_IP=`ip addr | grep 'state UP' -A2 | tail -n1 | awk '{print $2}' | awk -F"/" '{print $1}'`
+
+#if [ ! ${LOCAL_IP} ]; then
+#    echo "please check network"
+#    exit
+#fi
+
+LOCAL_IP=`ip addr | grep 'state UP' -A2 | tail -n1 | awk '{print $2}' | awk -F"/" '{print $1}'`
+echo "LOCAL_IP=\`ip addr | grep 'state UP' -A2 | tail -n1 | awk '{print \$2}' | awk -F"/" '{print \$1}'\`" >> ~/.pibotrc
+
+if [ ! ${LOCAL_IP} ]; then
+    echo -e "\033[1;31muse 127.0.0.1 as local ip\033[0m"
+    LOCAL_IP=127.0.0.1
+fi
+
+if [ "$PIBOT_MODEL" == "" ]; then
+    echo -e "\033[1;34mplease specify pibot model:\033[1;32m
+                            (0: apollo(2wd-diff),
+                            1: apolloX(2wd-diff),
+                            2: zeus(3wd-omni),
+                            3: hera(4wd-diff),
+                            4: hades(4wd-mecanum),
+                            5: hadesX(4wd-mecanum),
+                            other: user defined)\033[1;33m"
+    read -p "" PIBOT_INPUT
+
+    PIBOT_MODEL_TYPE="diff"
+
+    if [ "$PIBOT_INPUT" = "0" ]; then
+        PIBOT_MODEL='apollo'
+    elif [ "$PIBOT_INPUT" = "1" ]; then
+        PIBOT_MODEL='apolloX'
+    elif [ "$PIBOT_INPUT" = "2" ]; then
+        PIBOT_MODEL='zeus'
+        PIBOT_MODEL_TYPE="omni"
+    elif [ "$PIBOT_INPUT" = "3" ]; then
+        PIBOT_MODEL='hera'
+    elif [ "$PIBOT_INPUT" = "4" ]; then
+        PIBOT_MODEL='hades'
+        PIBOT_MODEL_TYPE="omni"
+    elif [ "$PIBOT_INPUT" = "5" ]; then
+        PIBOT_MODEL='hadesX'
+        PIBOT_MODEL_TYPE="omni"
+    else
+        PIBOT_MODEL=$PIBOT_INPUT 
+    fi
+fi
+
+if [ $PIBOT_MODEL == 'apollo' ] || [ $PIBOT_MODEL == 'apolloX' ] || [ $PIBOT_MODEL == 'hera' ]; then
+    PIBOT_MODEL_TYPE="diff"
+else
+    PIBOT_MODEL_TYPE="omni"
+fi
+
+if [ "$PIBOT_BOARD" == "" ]; then
+    echo -e "\033[1;34mplease specify pibot driver board type:\033[1;32m
+                            (0: arduino(mega2560),
+                            1: stm32f103,
+                            2: stm32f407,
+                            other: user defined)\033[1;33m"
+    read -p "" PIBOT_INPUT
+
+    if [ "$PIBOT_INPUT" = "0" ]; then
+        PIBOT_BOARD='arduino'
+    elif [ "$PIBOT_INPUT" = "1" ]; then
+        PIBOT_BOARD='stm32f1'
+    elif [ "$PIBOT_INPUT" = "2" ]; then
+        PIBOT_BOARD='stm32f4'
+    else
+        PIBOT_BOARD=$PIBOT_INPUT
+    fi
+fi
+
+if [ $PIBOT_BOARD == 'arduino' ] || [ $PIBOT_BOARD == 'stm32f1' ]; then
+    PIBOT_DRIVER_BAUDRATE=115200
+else
+    PIBOT_DRIVER_BAUDRATE=921600
+fi
+
+PIBOT_FAKE_LIDAR=0
+if [ "$PIBOT_LIDAR" == "" ]; then
+    echo -e "\033[1;34mplease specify your pibot lidar:\033[1;32m
+                            (0: not config,
+                            1: rplidar(a1,a2),
+                            2: rplidar(a3),
+                            3: eai(x4),
+                            4: eai(g4),
+                            5: eai(tg15/tg30/tg50),
+                            6: xtion,
+                            7: astra,
+                            8: kinectV1,
+                            9: kinectV2,
+                            10: rplidar(s1),
+                            other: user defined)\033[1;33m"
+    read -p "" PIBOT_INPUT
+
+    if [ "$PIBOT_INPUT" = "0" ]; then
+        PIBOT_LIDAR='none'
+    elif [ "$PIBOT_INPUT" = "1" ]; then
+        PIBOT_LIDAR='rplidar'
+    elif [ "$PIBOT_INPUT" = "2" ]; then
+        PIBOT_LIDAR='rplidar-a3'
+    elif [ "$PIBOT_INPUT" = "3" ]; then
+        PIBOT_LIDAR='eai-x4'
+    elif [ "$PIBOT_INPUT" = "4" ]; then
+        PIBOT_LIDAR='eai-g4'
+    elif [ "$PIBOT_INPUT" = "5" ]; then
+        PIBOT_LIDAR='eai-tgx'
+    elif [ "$PIBOT_INPUT" = "6" ]; then
+        PIBOT_LIDAR='xtion'
+        PIBOT_FAKE_LIDAR=1
+    elif [ "$PIBOT_INPUT" = "7" ]; then
+        PIBOT_LIDAR='astra'
+        PIBOT_FAKE_LIDAR=1
+    elif [ "$PIBOT_INPUT" = "8" ]; then
+        PIBOT_LIDAR='kinectV1'
+        PIBOT_FAKE_LIDAR=1
+    elif [ "$PIBOT_INPUT" = "9" ]; then
+        PIBOT_LIDAR='kinectV2'
+        PIBOT_FAKE_LIDAR=1
+    elif [ "$PIBOT_INPUT" = "10" ]; then
+        PIBOT_LIDAR='rplidar-s1'
+    else
+        PIBOT_LIDAR=$PIBOT_INPUT
+    fi
+fi
+
+if [ "$PIBOT_LIDAR" = "xtion" ]; then
+    PIBOT_FAKE_LIDAR=1
+elif [ "$PIBOT_LIDAR" = "astra" ]; then
+    PIBOT_FAKE_LIDAR=1
+elif [ "$PIBOT_LIDAR" = "kinectV1" ]; then
+    PIBOT_FAKE_LIDAR=1
+elif [ "$PIBOT_LIDAR" = "kinectV2" ]; then
+    PIBOT_FAKE_LIDAR=1
+    ln -s $PWD/third_party/iai_kinect2 $PWD/ros_ws/src/
+else
+    if [ -f $PWD/ros_ws/src/iai_kinect2 ]; then
+        rm $PWD/ros_ws/src/iai_kinect2
+    fi
+fi
+
+if [ $PIBOT_FAKE_LIDAR = 1 ]; then
+    echo "fake lidar: $PIBOT_LIDAR"
+    PIBOT_3DSENSOR='none'
+else
+    if [ "$PIBOT_3DSENSOR" == "" ]; then
+        echo -e "\033[1;34mplease specify your pibot 3dsensor:\033[1;32m
+                            (0: not config,
+                            1: xtion,
+                            2: astra,
+                            3: kinectV1,
+                            4: kinectV2,
+                            5: d435i,
+                            other: user defined)\033[1;33m"    
+        read -p "" PIBOT_INPUT
+
+        if [ "$PIBOT_INPUT" = "0" ]; then
+            PIBOT_3DSENSOR='none'
+        elif [ "$PIBOT_INPUT" = "1" ]; then
+            PIBOT_3DSENSOR='xtion'
+        elif [ "$PIBOT_INPUT" = "2" ]; then
+            PIBOT_3DSENSOR='astra'
+        elif [ "$PIBOT_INPUT" = "3" ]; then
+            PIBOT_3DSENSOR='kinectV1'
+        elif [ "$PIBOT_INPUT" = "4" ]; then
+            PIBOT_3DSENSOR='kinectV2'
+        elif [ "$PIBOT_INPUT" = "5" ]; then
+            PIBOT_3DSENSOR='d435i'
+        else
+            PIBOT_3DSENSOR=$PIBOT_INPUT
+        fi
+    fi
+fi
+
+echo "export PIBOT_HOME=${PIBOT_HOME_DIR}" >> ~/.pibotrc
+echo "export ROS_IP=\`echo \$LOCAL_IP\`" >> ~/.pibotrc
+echo "export ROS_HOSTNAME=\`echo \$LOCAL_IP\`" >> ~/.pibotrc
+echo "export PIBOT_MODEL=${PIBOT_MODEL}" >> ~/.pibotrc
+echo "export PIBOT_MODEL_TYPE=${PIBOT_MODEL_TYPE}" >> ~/.pibotrc
+echo "export PIBOT_LIDAR=${PIBOT_LIDAR}" >> ~/.pibotrc
+echo "export PIBOT_3DSENSOR=${PIBOT_3DSENSOR}" >> ~/.pibotrc
+echo "export PIBOT_BOARD=${PIBOT_BOARD}" >> ~/.pibotrc
+echo "export PIBOT_DRIVER_BAUDRATE=${PIBOT_DRIVER_BAUDRATE}" >> ~/.pibotrc
+echo "export PIBOT_MAPS_DIR=~/maps" >> ~/.pibotrc
+echo "export PIBOT_ASTRA_PID=0x\`lsusb | grep "2bc5:05" | awk '{print \$6}' | awk -F":" '{print \$2}'\`" >> ~/.pibotrc
+
+if [ "$PIBOT_ADDRESS" == "" ]; then
+    echo -e "\033[1;34mplease specify the current machine(ip:$LOCAL_IP) type:\033[1;32m
+                            (0: pibot board,
+                            \033[31m1: control PC or Virtual PC\033[1;34m)\033[1;33m" 
+    read PIBOT_INPUT
+elif [ $PIBOT_ADDRESS == $LOCAL_IP ] || [ $PIBOT_ADDRESS == "localhost" ]; then
+    PIBOT_INPUT="0"
+else
+    PIBOT_INPUT=$PIBOT_ADDRESS
+fi
+
+if [ "$PIBOT_INPUT" == "0" ]; then
+    ROS_MASTER_IP_STR="\`echo \$LOCAL_IP\`"
+    ROS_MASTER_IP=`echo $LOCAL_IP`
+else
+    if [ "$PIBOT_ADDRESS" != "$LOCAL_IP" ]; then
+        echo -e "\033[1;34mplase specify the pibot board ip for commnication(e.g. 192.168.12.1):" 
+        read -p "" PIBOT_INPUT
+    else
+        PIBOT_INPUT=$PIBOT_ADDRESS
+    fi
+    ROS_MASTER_IP_STR=`echo $PIBOT_INPUT`
+    ROS_MASTER_IP=`echo $PIBOT_INPUT`
+fi
+
+echo "export ROS_MASTER_URI=`echo http://${ROS_MASTER_IP_STR}:11311`" >> ~/.pibotrc
+
+echo -e "\033[1;35m*****************************************************************"
+echo "model:        " $PIBOT_MODEL 
+echo "lidar:        " $PIBOT_LIDAR  
+echo "local_ip:     " ${LOCAL_IP} 
+echo "onboard_ip:   " ${ROS_MASTER_IP}
+echo ""
+echo -e "please execute \033[1;36;4msource ~/.bashrc\033[1;35m to make the configure effective\033[0m"
+echo -e "\033[1;35m*****************************************************************\033[0m"
+
+#echo "source $PIBOT_HOME_DIR/ros_ws/devel/setup.bash" >> ~/.pibotrc 
+content="#source pibot
+if [ ! -f $PIBOT_HOME_DIR/ros_ws/devel/setup.bash ]; then 
+    echo -e \"please run \033[1;36;4mcd $PIBOT_HOME_DIR/ros_ws && catkin_make\033[0m to compile pibot sdk\"
+else
+    source $PIBOT_HOME_DIR/ros_ws/devel/setup.bash
+fi
+"
+echo "${content}" >> ~/.pibotrc
+
+#alias
+echo "alias pibot_bringup='roslaunch pibot_bringup bringup.launch'" >> ~/.pibotrc 
+echo "alias pibot_bringup_with_imu='roslaunch pibot_bringup bringup_with_imu.launch'" >> ~/.pibotrc 
+echo "alias pibot_lidar='roslaunch pibot_lidar ${PIBOT_LIDAR}.launch'" >> ~/.pibotrc 
+echo "alias pibot_base='roslaunch pibot_bringup robot.launch'" >> ~/.pibotrc 
+echo "alias pibot_base_with_imu='roslaunch pibot_bringup robot.launch use_imu:=true'" >> ~/.pibotrc 
+echo "alias pibot_control='roslaunch pibot_bringup keyboard_teleop.launch'" >> ~/.pibotrc 
+echo "alias pibot_joystick='roslaunch pibot_bringup joystick.launch'" >> ~/.pibotrc
+echo "alias pibot_holonomic_joystick='roslaunch pibot_bringup joystick.launch holonomic:=true'" >> ~/.pibotrc
+
+echo "alias pibot_configure='rosrun rqt_reconfigure rqt_reconfigure'" >> ~/.pibotrc 
+echo "alias pibot_simulator='roslaunch pibot_simulator nav.launch'" >> ~/.pibotrc 
+
+echo "alias pibot_gmapping='roslaunch pibot_navigation gmapping.launch'" >> ~/.pibotrc 
+echo "alias pibot_gmapping_with_imu='roslaunch pibot_navigation gmapping.launch use_imu:=true'" >> ~/.pibotrc 
+echo "alias pibot_save_map='roslaunch pibot_navigation save_map.launch'" >> ~/.pibotrc 
+
+echo "alias pibot_naviagtion='roslaunch pibot_navigation nav.launch'" >> ~/.pibotrc 
+echo "alias pibot_naviagtion_with_imu='roslaunch pibot_navigation nav.launch use_imu:=true'" >> ~/.pibotrc 
+echo "alias pibot_view='roslaunch pibot_navigation view_nav.launch'" >> ~/.pibotrc 
+
+echo "alias pibot_cartographer='roslaunch pibot_navigation cartographer.launch'" >> ~/.pibotrc 
+echo "alias pibot_cartographer_with_odom='roslaunch pibot_navigation cartographer_with_odom.launch'" >> ~/.pibotrc 
+echo "alias pibot_view_cartographer='roslaunch pibot_navigation view_cartographer.launch'" >> ~/.pibotrc 
+
+echo "alias pibot_hector_mapping='roslaunch pibot_navigation hector_mapping.launch'" >> ~/.pibotrc 
+echo "alias pibot_hector_mapping_without_imu='roslaunch pibot_navigation hector_mapping_without_odom.launch'" >> ~/.pibotrc 
+
+echo "alias pibot_karto_slam='roslaunch pibot_navigation karto_slam.launch'" >> ~/.pibotrc
+
+echo "alias pibot_3d_mapping='roslaunch pibot_slam_3d rtabmap.launch use_imu:=false localization:=false'" >> ~/.pibotrc
+echo "alias pibot_3d_mapping_with_imu='roslaunch pibot_slam_3d rtabmap.launch use_imu:=true localization:=false'" >> ~/.pibotrc
+echo "alias pibot_3d_nav='roslaunch pibot_slam_3d rtabmap.launch use_imu:=false localization:=true'" >> ~/.pibotrc
+echo "alias pibot_3d_nav_with_imu='roslaunch pibot_slam_3d rtabmap.launch use_imu:=true localization:=true'" >> ~/.pibotrc
+echo "alias pibot_3d_view_mapping='roslaunch pibot_slam_3d view.launch localization:=true'" >> ~/.pibotrc
+echo "alias pibot_3d_view_nav='roslaunch pibot_slam_3d view.launch localization:=false'" >> ~/.pibotrc

pibot_ros/pibot_install_ros.sh

@@ -0,0 +1,141 @@
+#!/bin/bash
+if [ ! -z "$PIBOT_HOME" ]; then
+    PIBOT_HOME_DIR=$PIBOT_HOME
+else
+    PIBOT_HOME_DIR=~/pibot_ros
+fi
+
+echo -e "\033[1;32mpibot home dir:$PIBOT_HOME_DIR"
+
+# http://wiki.ros.org/ROS/Installation/UbuntuMirrors
+sudo sh -c 'echo "deb http://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'
+sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" >> /etc/apt/sources.list.d/ros-latest.list'
+sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
+sudo apt-get update
+
+code_name=$(lsb_release -sc)
+
+if [ "$code_name" = "trusty" ]; then
+    ROS_DISTRO="indigo"
+elif [ "$code_name" = "xenial" ]; then
+    ROS_DISTRO="kinetic"
+elif [ "$code_name" = "bionic" ] || [ "$code_name" = "stretch" ]; then
+    ROS_DISTRO="melodic"
+elif [ "$code_name" = "focal" ] || [ "$code_name" = "buster" ]; then
+    ROS_DISTRO="noetic"
+else
+    echo "PIBOT not support "$code_name
+    exit
+fi
+
+echo "ros distro:" $ROS_DISTRO
+
+sudo apt-get install -y git cmake unzip vim build-essential udev inetutils-ping iproute2 hostapd
+            
+sudo apt-get -y --allow-unauthenticated install ros-${ROS_DISTRO}-ros-base ros-${ROS_DISTRO}-slam-gmapping ros-${ROS_DISTRO}-navigation \
+            ros-${ROS_DISTRO}-xacro ros-${ROS_DISTRO}-robot-state-publisher \
+            ros-${ROS_DISTRO}-joint-state-publisher ros-${ROS_DISTRO}-teleop-twist-* ros-${ROS_DISTRO}-control-msgs \
+            ros-${ROS_DISTRO}-kdl-parser-py ros-${ROS_DISTRO}-tf2-geometry-msgs ros-${ROS_DISTRO}-hector-mapping \
+            ros-${ROS_DISTRO}-robot-pose-ekf ros-${ROS_DISTRO}-slam-karto ros-${ROS_DISTRO}-hector-geotiff ros-${ROS_DISTRO}-hector-trajectory-server \
+            ros-${ROS_DISTRO}-usb-cam ros-${ROS_DISTRO}-image-transport ros-${ROS_DISTRO}-image-transport-plugins \
+            ros-${ROS_DISTRO}-depthimage-to-laserscan ros-${ROS_DISTRO}-openni2* \
+            ros-${ROS_DISTRO}-robot-upstart ros-${ROS_DISTRO}-tf-conversions \
+            ros-${ROS_DISTRO}-realsense2-camera ros-${ROS_DISTRO}-libuvc* \
+            ros-${ROS_DISTRO}-camera-calibration ros-${ROS_DISTRO}-rtabmap* \
+            ros-${ROS_DISTRO}-web-video-server ros-${ROS_DISTRO}-roslint ros-${ROS_DISTRO}-laser-filters
+            
+if [ "$ROS_DISTRO" = "noetic" ]; then
+    sudo ln -sf /usr/bin/python3 /usr/bin/python
+
+    # third 
+    cd $PIBOT_HOME_DIR/third_party/libuvc && mkdir -p build && cd build && cmake .. && sudo make install
+
+    cd $PIBOT_HOME_DIR
+    if [ ! -d $PWD/ros_package ]; then
+        mkdir ros_package
+    fi
+    cd ros_package
+
+    # astra ros package
+    if [ -f $PWD/astra.tar.gz ]; then
+        tar xzvf $PWD/astra.tar.gz
+    else
+        git clone https://github.com/orbbec/ros_astra_launch.git
+        git clone https://github.com/orbbec/ros_astra_camera.git
+    fi
+
+    # frontier_exploration ros package
+    # if [ -f $PWD/frontier_exploration.tar.gz ]; then
+    #     tar xzvf $PWD/frontier_exploration.tar.gz
+    # else
+    #     git clone -b $ROS_DISTRO-devel https://github.com/paulbovbel/frontier_exploration.git
+    # fi
+
+    # camera_umd
+    # if [ -f $PWD/camera_umd.tar.gz ]; then
+    #     tar xzvf $PWD/camera_umd.tar.gz
+    # else
+    #     git clone https://github.com/ros-drivers/camera_umd.git
+    # fi
+
+    cd ..
+    echo "ln -sf $PWD/ros_package ros_ws/src/ros_package"
+    if [ -f ros_ws/src/ros_package ]; then
+        rm ros_ws/src/ros_package
+    fi
+
+    ln -snf $PWD/ros_package ros_ws/src/ros_package
+    sudo apt-get -y --allow-unauthenticated install python3-pip python3-serial
+elif [ "$ROS_DISTRO" = "melodic" ]; then
+    # third 
+    cd $PIBOT_HOME_DIR/third_party/libuvc && mkdir -p build && cd build && cmake .. && sudo make install
+
+    cd $PIBOT_HOME_DIR
+    if [ ! -d $PWD/ros_package ]; then
+        mkdir ros_package
+    fi
+    cd ros_package
+
+    # astra ros package
+    if [ -f $PWD/astra.tar.gz ]; then
+        tar xzvf $PWD/astra.tar.gz
+    else
+        git clone https://github.com/orbbec/ros_astra_launch.git
+        git clone https://github.com/orbbec/ros_astra_camera.git
+    fi
+
+    # frontier_exploration ros package
+    # if [ -f $PWD/frontier_exploration.tar.gz ]; then
+    #     tar xzvf $PWD/frontier_exploration.tar.gz
+    # else
+    #     git clone -b $ROS_DISTRO-devel https://github.com/paulbovbel/frontier_exploration.git
+    # fi
+    cd ..
+
+    echo "ln -sf $PWD/ros_package ros_ws/src/ros_package"
+    if [ -f ros_ws/src/ros_package ]; then
+        rm ros_ws/src/ros_package
+    fi
+    
+    ln -snf $PWD/ros_package ros_ws/src/ros_package
+
+    sudo apt-get -y --allow-unauthenticated install python-pip python-serial \
+            ros-${ROS_DISTRO}-freenect-* ros-${ROS_DISTRO}-orocos-kdl ros-${ROS_DISTRO}-camera-umd ros-${ROS_DISTRO}-cartographer-ros
+else
+    sudo apt-get -y --allow-unauthenticated install python-pip python-serial \ 
+            ros-${ROS_DISTRO}-freenect-* ros-${ROS_DISTRO}-orocos-kdl ros-${ROS_DISTRO}-camera-umd ros-${ROS_DISTRO}-cartographer-ros\
+            ros-${ROS_DISTRO}-astra-launch ros-${ROS_DISTRO}-astra-camera ros-${ROS_DISTRO}-frontier-exploration
+fi
+
+read -s -n1 -p "install ros gui tools?(y/N)" 
+
+if [ "$REPLY" = "y" -o "$REPLY" = "Y" ]; then
+    sudo apt-get -y --allow-unauthenticated install ros-${ROS_DISTRO}-rviz ros-${ROS_DISTRO}-rqt-reconfigure ros-${ROS_DISTRO}-rqt-tf-tree \
+    ros-${ROS_DISTRO}-image-view
+
+    if [ "$ROS_DISTRO" = "noetic" ]; then
+        echo "please run ros_package/make_cartographer.sh to compile cartographer"
+    else
+        sudo apt-get -y --allow-unauthenticated install ros-${ROS_DISTRO}-cartographer-rviz
+    fi
+fi

pibot_ros/pibot_remove_upstart.sh

@@ -0,0 +1,4 @@
+#!/bin/bash
+
+sudo systemctl disable pibot
+sudo systemctl is-enabled pibot

pibot_ros/pibot_upstart/pibot.service

@@ -0,0 +1,11 @@
+[Unit]
+Description=this is pibot service
+
+[Service]
+Type=simple
+ExecStart=/usr/bin/pibot_start
+ExecStop=/usr/bin/pibot_stop
+ExecRestart=/usr/bin/pibot_restart
+
+[Install]
+WantedBy=multi-user.target

pibot_ros/pibot_upstart/pibot_restart.sh

@@ -0,0 +1,4 @@
+#!/bin/bash
+/usr/bin/pibot-stop
+sleep 2
+/usr/bin/pibot-start 

pibot_ros/pibot_upstart/pibot_start.sh

@@ -0,0 +1,33 @@
+#!/bin/bash
+
+log_file=/tmp/pibot-upstart.log
+echo "$DATE: pibot-start" >> $log_file
+pibotenv=/etc/pibotenv
+. $pibotenv
+
+code_name=$(lsb_release -sc)
+
+if [ "$code_name" = "trusty" ]; then
+    ROS_DISTRO="indigo"
+elif [ "$code_name" = "xenial" ]; then
+    ROS_DISTRO="kinetic"
+elif [ "$code_name" = "bionic" ] || [ "$code_name" = "stretch" ]; then
+    ROS_DISTRO="melodic"
+elif [ "$code_name" = "focal" ] || [ "$code_name" = "buster" ]; then
+    ROS_DISTRO="noetic"
+else
+    echo "PIBOT not support $code_name" >> $log_file
+    exit
+fi
+
+echo "SYS_DIST:                $code_name" >> $log_file
+echo "ROS_DIST:                $ROS_DISTRO" >> $log_file
+echo "LOCAL_IP:                $LOCAL_IP" >> $log_file
+echo "ROS_MASTER_URI:          $ROS_MASTER_URI" >> $log_file
+echo "ROS_IP:                  $ROS_IP" >> $log_file
+echo "HOSTNAME:                $ROS_HOSTNAME" >> $log_file
+echo "PIBOT_MODEL:             $PIBOT_MODEL" >> $log_file
+echo "PIBOT_LIDAR:             $PIBOT_LIDAR" >> $log_file
+echo "PIBOT_BOARD:             $PIBOT_BOARD" >> $log_file
+
+roslaunch pibot_navigation gmapping.launch

pibot_ros/pibot_upstart/pibot_stop.sh

@@ -0,0 +1,38 @@
+#!/bin/bash
+log_file=/tmp/pibot-upstart.log
+echo "$DATE: pibot-stop" >> $log_file
+pibotenv=/etc/pibotenv
+. $pibotenv
+
+code_name=$(lsb_release -sc)
+
+if [ "$code_name" = "trusty" ]; then
+    ROS_DISTRO="indigo"
+elif [ "$code_name" = "xenial" ]; then
+    ROS_DISTRO="kinetic"
+elif [ "$code_name" = "bionic" ] || [ "$code_name" = "stretch" ]; then
+    ROS_DISTRO="melodic"
+elif [ "$code_name" = "focal" ] || [ "$code_name" = "buster" ]; then
+    ROS_DISTRO="noetic"
+else
+    echo "PIBOT not support $code_name" >> $log_file
+    exit
+fi
+
+LOCAL_IP=`ip addr | grep 'state UP' -A2 | tail -n1 | awk '{print $2}' | awk -F"/" '{print $1}'`
+echo $LOCAL_IP
+echo "SYS_DIST:                $code_name" >> $log_file
+echo "ROS_DIST:                $ROS_DISTRO" >> $log_file
+echo "LOCAL_IP:                $LOCAL_IP" >> $log_file
+echo "ROS_MASTER_URI:          $ROS_MASTER_URI" >> $log_file
+echo "ROS_IP:                  $ROS_IP" >> $log_file
+echo "HOSTNAME:                $ROS_HOSTNAME" >> $log_file
+echo "PIBOT_MODEL:             $PIBOT_MODEL" >> $log_file
+echo "PIBOT_LIDAR:             $PIBOT_LIDAR" >> $log_file
+echo "PIBOT_BOARD:             $PIBOT_BOARD" >> $log_file
+
+for i in $( rosnode list ); do
+    rosnode kill $i;
+done
+
+killall roslaunch

pibot_ros/pibot_upstart/pibotenv

@@ -0,0 +1,22 @@
+#source ros
+source /opt/ros/kinetic/setup.bash
+
+if [ ! -z "$PIBOT_HOME" ]; then
+    PIBOT_HOME_DIR=$PIBOT_HOME
+else
+    PIBOT_HOME_DIR=/home/pibot/pibot_ros
+fi
+
+LOCAL_IP=192.168.12.1
+export ROS_IP=$LOCAL_IP
+export ROS_HOSTNAME=$LOCAL_IP
+export PIBOT_MODEL=hades
+export PIBOT_LIDAR=rplidar
+export PIBOT_3DSENSOR=none
+export PIBOT_BOARD=stm32f4
+export PIBOT_DRIVER_BAUDRATE=921600
+export ROS_MASTER_URI=http://$LOCAL_IP:11311
+
+#source pibot
+source $PIBOT_HOME_DIR/ros_ws/devel/setup.bash
+

pibot_ros/pibot_view_env.sh

@@ -0,0 +1,31 @@
+#!/bin/bash
+
+code_name=$(lsb_release -sc)
+
+if [ "$code_name" = "trusty" ]; then
+    ROS_DISTRO="indigo"
+elif [ "$code_name" = "xenial" ]; then
+    ROS_DISTRO="kinetic"
+elif [ "$code_name" = "bionic" ] || [ "$code_name" = "stretch" ]; then
+    ROS_DISTRO="melodic"
+elif [ "$code_name" = "focal" ] || [ "$code_name" = "buster" ]; then
+    ROS_DISTRO="noetic"
+else
+    echo -e "\033[1;31m PIBOT not support "$code_name"\033[0m"
+    exit
+fi 
+
+LOCAL_IP=`ip addr | grep 'state UP' -A2 | tail -n1 | awk '{print $2}' | awk -F"/" '{print $1}'`
+
+echo -e "\033[1;33mPIBOT_ENV_INITIALIZED:   "$PIBOT_ENV_INITIALIZED
+echo -e "\033[1;34mSYS_DIST:                "$code_name
+echo -e "\033[1;33mROS_DIST:                "$ROS_DISTRO
+echo -e "\033[1;34mLOCAL_IP:                "$LOCAL_IP
+echo -e "\033[1;33mROS_MASTER_URI:          "$ROS_MASTER_URI
+echo -e "\033[1;34mROS_IP:                  "$ROS_IP
+echo -e "\033[1;33mROS_HOSTNAME:            "$ROS_HOSTNAME
+echo -e "\033[1;34mPIBOT_MODEL:             "$PIBOT_MODEL
+echo -e "\033[1;33mPIBOT_LIDAR:             "$PIBOT_LIDAR
+echo -e "\033[1;34mPIBOT_BOARD:             "$PIBOT_BOARD
+echo -e "\033[1;33mPIBOT_3DSENSOR:          "$PIBOT_3DSENSOR
+echo -e "\033[1;35m*****************************************************************\033[0m"

pibot_ros/pypibot/pypibot/__init__.py

@@ -0,0 +1,17 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+import pypibot.assistant
+import pypibot.log as Logger
+import pypibot.err
+log=Logger.log
+import sys
+def createNamedLog(name):
+    return Logger.NamedLog(name)
+class Object():
+    pass
+isDebug="-d" in sys.argv
+
+import platform
+isWindows=False
+if platform.system()=='Windows':
+    isWindows=True

pibot_ros/pypibot/pypibot/assistant.py

@@ -0,0 +1,234 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+import os, sys, inspect
+import datetime
+import signal
+import threading
+import time
+# function: get directory of current script, if script is built
+#   into an executable file, get directory of the excutable file
+def current_file_directory():
+    path = os.path.realpath(sys.path[0])        # interpreter starter's path
+    if os.path.isfile(path):                    # starter is excutable file
+        path = os.path.dirname(path)
+        path = os.path.abspath(path)            # return excutable file's directory
+    else:                                       # starter is python script
+        caller_file = inspect.stack()[0][1]     # function caller's filename
+        path = os.path.abspath(os.path.dirname(caller_file))# return function caller's file's directory
+    if path[-1]!=os.path.sep:path+=os.path.sep
+    return path
+
+"""格式化字符串"""
+def formatString(string,*argv):
+    string=string%argv
+    if string.find('$(scriptpath)')>=0:
+        string=string.replace('$(scriptpath)',current_file_directory())
+    if string.find('$(filenumber2)')>=0:
+        i=0
+        path=""
+        while True:
+            path=string.replace('$(scriptpath)',"%02d"%i)
+            if not os.path.lexists(path):break
+            i+=1
+        string=path
+    #8位日期（20140404）
+    if string.find('$(Date8)')>=0:
+        now=datetime.datetime.now()
+        string=string.replace('$(Date8)', now.strftime("%Y%m%d"))
+    #6位日期（140404）
+    if string.find('$(Date6)')>=0:
+        now=datetime.datetime.now()
+        string=string.replace('$(Date6)', now.strftime("%y%m%d"))
+    #6位时间（121212）
+    if string.find('$(Time6)')>=0:
+        now=datetime.datetime.now()
+        string=string.replace('$(Time6)', now.strftime("%H%M%S"))
+    #4位时间（1212）
+    if string.find('$(Time4)')>=0:
+        now=datetime.datetime.now()
+        string=string.replace('$(Time4)', now.strftime("%H%M"))
+    #文件编号2位（必须在最后）
+    if string.find('$(filenumber2)')>=0:
+        i=0
+        path=""
+        while True:
+            path=string.replace('$(filenumber2)',"%02d"%i)
+            if not os.path.lexists(path):break
+            i+=1
+        string=path
+    #文件编号3位（必须在最后）
+    if string.find('$(filenumber3)')>=0:
+        i=0
+        path=""
+        while True:
+            path=string.replace('$(filenumber3)',"%03d"%i)
+            if not os.path.lexists(path):break
+            i+=1
+        string=path
+    return string
+
+"""
+    取得进程列表
+    格式：(PID,cmd)
+""" 
+def getProcessList():
+    processList = []
+    try:
+        for line in os.popen("ps xa"):
+            fields = line.split()
+            # spid = fields[0]
+            pid = 0
+            try:pid = int(fields[0])
+            except:None
+            cmd = line[27:-1]
+            # print "PS:%d,%s"%(npid,process)
+            if pid != 0 and len(cmd) > 0:
+                processList.append((pid, cmd))
+    except Exception as e:
+        print("getProcessList except:%s" % (e))
+    return processList
+def killCommand(cmd):
+    try:
+        processList = getProcessList()
+        for p in processList:
+            if p[1].find(cmd) != -1:
+                pid = p[0]
+                os.kill(pid, signal.SIGKILL)
+    except Exception as e:
+        print("killCommand ‘%s’ except:%s" % (cmd,e))
+
+def check_pid(pid):        
+    """ Check For the existence of a unix pid. """
+    if pid == 0:return False
+    try:
+        os.kill(pid, 0)
+    except OSError:
+        return False
+    else:
+        return True
+
+SF=formatString
+
+#全局异常捕获
+def excepthook(excType, excValue, tb):  
+    '''''write the unhandle exception to log''' 
+    from log import log
+    import traceback
+    log.e('Unhandled Error: %s',''.join(traceback.format_exception(excType, excValue, tb)))
+    sys.exit(-1)
+    #sys.__excepthook__(type, value, trace)
+    #sys.__excepthook__(excType, excValue, tb)
+
+_defaultGlobalExcept=sys.excepthook
+
+def enableGlobalExcept(enable=True):
+    if enable:
+        sys.excepthook = excepthook
+    else:
+        sys.excepthook=_defaultGlobalExcept
+# 默认启动全局异常处理
+enableGlobalExcept()
+#创建线程
+def createThread(name,target,args=(),autoRun=True,daemon=True):
+    from log import log
+    def threadProc():
+        log.i("thread %s started!",name)
+        try:
+            target(*args)
+            log.i("thread %s ended!",name)
+        except Exception as e:
+            log.e("thread %s crash!err=%s",name,e)
+    thd=threading.Thread(name=name,target=threadProc)
+    thd.setDaemon(daemon)
+    if autoRun:thd.start()
+    return thd
+
+
+#定时器
+class Timer():
+    def __init__(self, timer_proc, args=(),first=0,period=0,name="Timer"):
+        self.name=name
+        self.first=first
+        self.period=period
+        self.args=args
+        self.timer_proc=timer_proc
+        self.thdWork=None
+        self.stopFlag=False
+        from log import NamedLog
+        self.log=NamedLog(name)
+    def run(self):
+        if self.first>0:
+            time.sleep(self.first)
+            try:
+                self.timer_proc(*self.args)
+            except Exception as e:
+                self.log.error("timer proc crash!err=%s",e)
+        while self.period>0 and not self.stopFlag:
+            time.sleep(self.period)
+            try:
+                self.timer_proc(*self.args)
+            except Exception as e:
+                self.log.error("timer proc crash!err=%s",e)
+    def start(self):
+        if self.isAlive():
+            self.log.d("already running!")
+            return True
+        self.stopFlag=False
+        self.thdWork=threading.Thread(name=self.name,target=self.run)
+        self.thdWork.setDaemon(True)
+        self.thdWork.start()
+    def stop(self,timeout=3):
+        if self.isAlive():
+            self.stopFlag=True
+            try:
+                self.thdWork.join(timeout)
+            except Exception as e:
+                self.log.e("stop timeout!")
+
+    def isAlive(self):
+        return self.thdWork and self.thdWork.isAlive()
+#计时器
+class Ticker():
+    def __init__(self):
+        self.reset()
+        # 片段,可以判断时长是否在一个特定毫秒段内
+        self.section=[]
+    def reset(self):
+        self.tick=time.time()
+        self.end=0
+    def stop(self):
+        self.end=time.time()
+    @property
+    def ticker(self):
+        if self.end==0:
+            return int((time.time()-self.tick)*1000)
+        else:
+            return int((self.end-self.tick)*1000)
+    def addSection(self,a,b):
+        a,b=int(a),int(b)
+        assert a<b
+        self.section.append((a,b))
+    def removeSection(self,a,b):
+        a,b=int(a),int(b)
+        self.section.remove((a,b))
+    def removeAllSectioin(self):
+        self.section=[]
+    def inSection(self):
+        tick=self.ticker
+        for (a,b) in self.section:
+            if tick>=a and tick<=b:
+                return True
+        return False
+    def __call__(self):
+        return self.ticker
+def waitExit():
+    import log
+    log.log.i("start waiting to exit...")
+    try:
+        while(True):
+            time.sleep(1)
+    except Exception as e:
+        log.log.w("recv exit sign!")
+
+def is_python3():
+    return sys.hexversion > 0x03000000

pibot_ros/pypibot/pypibot/configer.py

@@ -0,0 +1,56 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+import ConfigParser
+from log import PLOG
+import os
+def getdefaultfilename():
+    pass
+def openconfiger(filename):
+    return configer(filename)
+class configer:
+    def __init__(self,fullfilepath):
+        self._filepath=fullfilepath
+        if not os.path.isdir(os.path.dirname(fullfilepath)):
+            os.makedirs(os.path.dirname(fullfilepath))
+        self._conf=ConfigParser.ConfigParser()
+        if os.path.isfile(fullfilepath):
+            try:
+                self._conf.readfp(open(fullfilepath,"r"))
+            except Exception,e:
+                PLOG.error("配置文件'%s'打开失败,err=%s"%(self._filepath,e))
+    def save(self):
+        try:
+            self._conf.write(open(self._filepath,"w"))
+        except Exception,e:
+            PLOG.error("配置文件'%s'保存失败,err=%s"%(self._filepath,e))
+            
+    def changeConfValue(self,section,option,value):
+        if self._conf.has_section(section):
+            self._conf.set(section,option,value)
+        else:
+            self._conf.add_section(section)
+            self._conf.set(section,option,value)
+            
+    def _readvalue(self,fn,section,option,default):
+        result=default
+        if self._conf.has_section(section):
+            if self._conf.has_option(section,option):
+                result=fn(section,option)
+                PLOG.debug("Option[%s][%s]=%s"%(section,option,str(result)))
+            else:
+                self._conf.set(section,option,str(default))
+                result=default
+        else:
+            self._conf.add_section(section)
+            self._conf.set(section,option,str(default))
+            result=default
+        return result
+    def getstr(self,section,option,default=""):
+        return self._readvalue(self._conf.get, section, option, default)
+    def getint(self,section,option,default=0):
+        return self._readvalue(self._conf.getint, section, option, default)
+    def getfloat(self,section,option,default=0.0):
+        return self._readvalue(self._conf.getfloat, section, option, default)
+    def getboolean(self,section,option,default=False):
+        return self._readvalue(self._conf.getboolean, section, option, default)

pibot_ros/pypibot/pypibot/daemon.py

@@ -0,0 +1,140 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import sys, os, time, atexit
+from signal import SIGTERM
+
+def check_pid(pid):        
+    """ Check For the existence of a unix pid. """
+    if pid == 0:return False
+    try:
+        os.kill(pid, 0)
+    except OSError:
+        return False
+    else:
+        return True
+    
+class daemon:
+    """
+    A generic daemon class.
+    
+    Usage: subclass the Daemon class and override the run() method
+    """
+    def __init__(self, pidfile, stdin='/dev/null', stdout='/dev/null', stderr='/dev/null'):
+        self.stdin = stdin
+        self.stdout = stdout
+        self.stderr = stderr
+        self.pidfile = pidfile
+    
+    def daemonize(self):
+        """
+        do the UNIX double-fork magic, see Stevens' "Advanced 
+        Programming in the UNIX Environment" for details (ISBN 0201563177)
+        http://www.erlenstar.demon.co.uk/unix/faq_2.html#SEC16
+        """
+        try: 
+            pid = os.fork() 
+            if pid > 0:
+                # exit first parent
+                sys.exit(0) 
+        except OSError, e: 
+            sys.stderr.write("fork #1 failed: %d (%s)\n" % (e.errno, e.strerror))
+            sys.exit(1)
+    
+        # decouple from parent environment
+        os.chdir("/") 
+        os.setsid() 
+        os.umask(0) 
+    
+        # do second fork
+        try: 
+            pid = os.fork() 
+            if pid > 0:
+                # exit from second parent
+                sys.exit(0) 
+        except OSError, e: 
+            sys.stderr.write("fork #2 failed: %d (%s)\n" % (e.errno, e.strerror))
+            sys.exit(1) 
+    
+        # redirect standard file descriptors
+        sys.stdout.flush()
+        sys.stderr.flush()
+        si = file(self.stdin, 'r')
+        so = file(self.stdout, 'a+')
+        se = file(self.stderr, 'a+', 0)
+        os.dup2(si.fileno(), sys.stdin.fileno())
+        os.dup2(so.fileno(), sys.stdout.fileno())
+        os.dup2(se.fileno(), sys.stderr.fileno())
+    
+        # write pidfile
+        atexit.register(self.delpid)
+        pid = str(os.getpid())
+        file(self.pidfile, 'w+').write("%s\n" % pid)
+    
+    def delpid(self):
+        os.remove(self.pidfile)
+
+    def start(self):
+        """
+        Start the daemon
+        """
+        # Check for a pidfile to see if the daemon already runs
+        try:
+            pf = file(self.pidfile, 'r')
+            pid = int(pf.read().strip())
+            pf.close()
+        except IOError:
+            pid = None
+    
+        if pid and check_pid(pid):
+            message = "pidfile %s already exist. Daemon already running?\n"
+            sys.stderr.write(message % self.pidfile)
+            sys.exit(1)
+        print("daemon start...")
+        # Start the daemon
+        self.daemonize()
+        self.run()
+
+    def stop(self):
+        """
+        Stop the daemon
+        """
+        # Get the pid from the pidfile
+        try:
+            pf = file(self.pidfile, 'r')
+            pid = int(pf.read().strip())
+            pf.close()
+        except IOError:
+            pid = None
+    
+        if not pid:
+            message = "pidfile %s does not exist. Daemon not running?\n"
+            sys.stderr.write(message % self.pidfile)
+            return  # not an error in a restart
+
+        # Try killing the daemon process    
+        try:
+            while 1:
+                os.kill(pid, SIGTERM)
+                time.sleep(0.1)
+        except OSError, err:
+            err = str(err)
+            if err.find("No such process") > 0:
+                if os.path.exists(self.pidfile):
+                    os.remove(self.pidfile)
+            else:
+                print(str(err))
+                sys.exit(1)
+
+    def restart(self):
+        """
+        Restart the daemon
+        """
+        self.stop()
+        self.start()
+
+    def run(self):
+        """
+        You should override this method when you subclass Daemon. It will be called after the process has been
+        daemonized by start() or restart().
+        """

pibot_ros/pypibot/pypibot/err.py

@@ -0,0 +1,58 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+# 异常类
+class PibotError(Exception):
+    def __init__(self, errcode, errmsg):
+        self.errcode = errcode
+        self.errmsg = errmsg
+        #Exception.__init__(self,self.__str__())
+
+    def msg(self, msg):
+        if not msg is None:return PibotError(self.errcode, msg)
+        return PibotError(8,"unknow error")
+    def __str__(self):
+        return "PibotError:%s(%d)"%(self.errmsg,self.errcode)
+    @property
+    def message(self):
+        return str(self)
+# 声明
+# 成功
+success=PibotError(0,"null")
+# 通用失败
+fail=PibotError(1,"fail")
+# 参数无效
+invalidParameter=PibotError(2,"invalid parameter")
+# 不支持
+noSupport=PibotError(3,"no support")
+# 不存在
+noExist=PibotError(4,"no exist")
+# 超时
+timeout=PibotError(5,"timeout")
+# 繁忙
+busy=PibotError(6,"busy")
+# 缺少参数
+missParameter=PibotError(7,"miss parameter")
+# 系统错误(通用错误)
+systemError=PibotError(8,"system error")
+# 密码错误
+invalidPassword=PibotError(9,"invalid password")
+# 编码失败
+encodeFailed=PibotError(10,"encode failed")
+# 数据库操作失败
+dbOpertationFailed=PibotError(11,"db error")
+# 已占用
+occupied=PibotError(12,"occupied")
+# session不存在
+noSession = PibotError(13,'cannot find session')
+#没有找到
+noFound = PibotError(14, "no found")
+#已经存在
+existed = PibotError(15, "existed")
+#已经锁定
+locked = PibotError(16, "locked")
+#已经过期
+expired = PibotError(17, "is expired")
+#无效的参数
+invalidParameter = PibotError(18, "invalid parameter")
+

pibot_ros/pypibot/pypibot/log.py

@@ -0,0 +1,259 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+import sys,os
+import datetime
+import threading
+import pypibot.assistant as assistant
+import platform
+if assistant.is_python3():
+    import _thread
+else:
+    import thread
+import traceback
+"""
+%a Locale’s abbreviated weekday name.   
+%A Locale’s full weekday name.   
+%b Locale’s abbreviated month name.   
+%B Locale’s full month name.   
+%c Locale’s appropriate date and time representation.   
+%d Day of the month as a decimal number [01,31].   
+%H Hour (24-hour clock) as a decimal number [00,23].   
+%I Hour (12-hour clock) as a decimal number [01,12].   
+%j Day of the year as a decimal number [001,366].   
+%m Month as a decimal number [01,12].   
+%M Minute as a decimal number [00,59].   
+%p Locale’s equivalent of either AM or PM. (1) 
+%S Second as a decimal number [00,61]. (2) 
+%U Week number of the year (Sunday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Sunday are considered to be in week 0. (3) 
+%w Weekday as a decimal number [0(Sunday),6].   
+%W Week number of the year (Monday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Monday are considered to be in week 0. (3) 
+%x Locale’s appropriate date representation.   
+%X Locale’s appropriate time representation.   
+%y Year without century as a decimal number [00,99].   
+%Y Year with century as a decimal number.   
+%Z Time zone name (no characters if no time zone exists).   
+%% A literal '%' character. 
+
+"""
+isWindows=False
+if platform.system()=='Windows':
+    isWindows=True
+defaultEncodeing="utf8"
+if "-utf8" in sys.argv:
+    defaultEncodeing="utf-8"
+if "-gbk" in sys.argv:
+    defaultEncodeing="gbk"
+
+TRACE=5
+DEBUG=4
+INFORMATION=3
+WARNING=2
+ERROR=1
+NONE=0
+
+MAX_MSG_SIZE = 4096
+
+def getLevelFromString(level):
+    level=level.lower()
+    if level=='t' or level=='trace':return 5
+    elif level=='d' or level=='debug':return 4
+    elif level=='i' or level=='info':return 3
+    elif level=='w' or level=='wran':return 2
+    elif level=='e' or level=='error':return 1
+    else :return 0
+def getLevelString(level):
+    if level==TRACE:return "T"
+    elif level==DEBUG:return "D"
+    elif level==INFORMATION:return "I"
+    elif level==WARNING:return "W"
+    elif level==ERROR:return "E"
+    else:return "N"
+class PibotLog:
+    def __init__(self):
+        self.isEnableControlLog=True
+        self.fileTemple=None
+        self.filePath=""
+        self.level=5
+        self._lock=threading.RLock()
+        self.fd=None
+        self.fd_day=-1
+    def setLevel(self,level):
+        self.level=getLevelFromString(level)
+    def enableControllog(self,enable):
+        self.isEnableControlLog=enable
+    def enableFileLog(self,fileName):
+        self.fileTemple=fileName
+        self.updateFilelog()
+    def updateFilelog(self):
+        fn=assistant.SF(self.fileTemple)
+        if fn!=self.filePath:
+            self.i("new log file:%s",fn)
+            if self.fd:
+                self.fd.close()
+                self.fd=None
+                self.fd_day=-1
+                self.filePath=""
+            try:
+                path=os.path.dirname(fn)
+                if path!="" and not os.path.isdir(path):os.makedirs(path)
+                self.fd=open(fn,mode="w")
+                try:
+                    linkfn = fn.split(".log.", 1)[0] + ".INFO"
+                    if os.path.exists(linkfn):
+                        os.remove(linkfn)
+                    (filepath, tempfilename) = os.path.split(fn)
+                    os.symlink(tempfilename, linkfn)
+                except:
+                    pass
+                self.fd_day=datetime.datetime.now().day
+                self.filePath=fn
+            except Exception as e:
+                print("open file fail!file=%s,err=%s"%(fn,e))
+    def _output(self,level,msg,args):
+        if self.level<level:return
+        try:
+            if len(args)>0:
+                t=[]
+                for arg in args:
+                    if isinstance(arg,Exception):
+                        t.append(traceback.format_exc(arg).decode('utf-8'))
+                    elif isinstance(arg,bytes) :
+                        t.append(arg.decode('utf-8'))
+                    else:
+                        t.append(arg) 
+                args=tuple(t)
+                try:
+                    msg=msg%args
+                except:
+                    try:
+                        for arg in args:
+                            msg=msg+str(arg)+" "
+                    except Exception as e:
+                        msg=msg+"==LOG ERROR==>%s"%(e)
+            if len(msg)>MAX_MSG_SIZE:msg=msg[0:MAX_MSG_SIZE]
+            now=datetime.datetime.now()
+            msg=msg+"\r\n"
+            if assistant.is_python3():
+                id = _thread.get_ident()
+            else:
+                id = thread.get_ident()
+            s="[%s] %04d-%02d-%02d %02d:%02d:%02d.%03d (0x%04X):%s"%(getLevelString(level),now.year,now.month,now.day,now.hour,now.minute,now.second,now.microsecond/1000,(id>>8)&0xffff,msg)
+            prefix=''
+            suffix=''
+            if not isWindows:
+                suffix='\033[0m'
+                if level==TRACE:
+                    prefix='\033[0;37m'
+                elif level==DEBUG:
+                    prefix='\033[1m'
+                elif level==INFORMATION:
+                    prefix='\033[0;32m'          
+                elif level==WARNING:
+                    prefix='\033[0;33m'          
+                elif level==ERROR:
+                    prefix='\033[0;31m'          
+                else:
+                    pass
+            self._lock.acquire()
+            try:
+                if self.isEnableControlLog:
+                    sys.stdout.write((prefix+s+suffix))
+                if self.fd:
+                    if self.fd_day!=now.day:
+                        self.updateFilelog()
+                    if assistant.is_python3():
+                        self.fd.write(s)
+                    else:
+                        self.fd.write(s.encode('utf-8'))
+                    self.fd.flush()
+            finally:
+                self._lock.release()
+        except Exception as e:
+            if assistant.is_python3():
+                print(e)
+            else:
+                print("PibotLog._output crash!err=%s"%traceback.format_exc(e))
+
+    def trace(self,msg,*args):
+        self._output(TRACE,msg,args)
+    def t(self,msg,*args):
+        self._output(TRACE,msg,args)
+    def debug(self,msg,*args):
+        self._output(DEBUG, msg,args)
+    def d(self,msg,*args):
+        self._output(DEBUG, msg,args)
+    def info(self,msg,*args):
+        self._output(INFORMATION, msg,args)
+    def i(self,msg,*args):
+        self._output(INFORMATION, msg,args)
+    def warn(self,msg,*args):
+        self._output(WARNING, msg,args)
+    def w(self,msg,*args):
+        self._output(WARNING, msg,args)
+    def error(self,msg,*args):
+        self._output(ERROR, msg,args)
+    def e(self,msg,*args):
+        self._output(ERROR, msg,args)
+    def _log(self,level,msg,args):
+        self._output(level, msg,args)
+    def createNamedLog(self,name):
+        return NamedLog(name)
+log=PibotLog()
+class NamedLog:
+    def __init__(self,name=None):
+        global log
+        self.name=''
+        if name:
+            self.name='['+name+']'
+        self.log=log
+    def trace(self,msg,*args):
+        msg=self.name+msg
+        self.log._log(TRACE,msg,args)
+    def t(self,msg,*args):
+        msg=self.name+msg
+        self.log._log(TRACE,msg,args)
+    def debug(self,msg,*args):
+        msg=self.name+msg
+        self.log._log(DEBUG, msg,args)
+    def d(self,msg,*args):
+        msg=self.name+msg
+        self.log._log(DEBUG, msg,args)
+    def info(self,msg,*args):
+        msg=self.name+msg
+        self.log._log(INFORMATION, msg,args)
+    def i(self,msg,*args):
+        msg=self.name+msg
+        self.log._log(INFORMATION, msg,args)
+    def warn(self,msg,*args):
+        msg=self.name+msg
+        self.log._log(WARNING, msg,args)
+    def w(self,msg,*args):
+        msg=self.name+msg
+        self.log._log(WARNING, msg,args)
+    def error(self,msg,*args):
+        msg=self.name+msg
+        self.log._log(ERROR, msg,args)
+    def e(self,msg,*args):
+        msg=self.name+msg
+        self.log._log(ERROR, msg,args)
+    
+if __name__ == "__main__":
+    log.trace("1%s","hello")
+    log.debug("2%d",12)
+    try:
+        raise Exception("EXC")
+    except Exception as e:
+        log.info("3%s",e)
+    log.warn("1")
+    log.error("1")
+    #log.enableFileLog("$(scriptpath)test_$(Date8)_$(filenumber2).log")
+    log.trace("1")
+    log.debug("1")
+    log.info("1")
+    log.warn("1")
+    log.error("1")
+    log=NamedLog("test")
+    log.d("1")
+    log.i("1")
+    log.warn("1")
+    log.error("1=%d,%s",100,e)

pibot_ros/pypibot/pypibot/mapconvert.py

@@ -0,0 +1,35 @@
+import cv2
+import numpy as np
+
+def map_server_to_cv2(data):
+    # print(data.info.height, data.info.width)
+    data = np.reshape(data.data, (data.info.height, data.info.width))
+    data = np.flipud(data)
+
+    # -1 --> 205
+    #  0 --> 255
+    # 100--> 0
+    image = np.where(data == -1, 205, 255 - (255.0 * data / 100)).astype(np.uint8)
+
+    #cv2.imshow("cv2_map", data)
+    return image
+
+
+def cv2_to_map_server(image):
+    image = np.flipud(image)
+
+    # 205 --> -1
+    # 255 -->  0
+    #   0 --> 100
+    data = np.where(image == 205, -1, (255 - image) * 100.0 / 255).astype(np.int8).flatten()
+    
+    return list(data)
+    # pub_data = OccupancyGrid()
+    # pub_data.header.frame_id = "map"
+    # pub_data.header.stamp = rospy.Time.now()
+    # pub_data.info = map_data.info
+    # pub_data.data = list(data)
+
+    # print(len(pub_data.data), pub_data.info.height * pub_data.info.width)
+
+    # map_publisher.publish(pub_data)

pibot_ros/pypibot/pypibot/proxy.py

@@ -0,0 +1,38 @@
+from pypibot import log
+
+import threading
+import zmq
+
+class MqProxy:
+    def __init__(self, sub_addr, pub_addr):
+        self.thd = None
+        self.sub_addr = sub_addr
+        self.pub_addr = pub_addr
+
+    def _run(self):
+        context = zmq.Context()
+
+        frontend = context.socket(zmq.XSUB)
+        frontend.bind(self.sub_addr)
+        # frontend.bind("tcp://*:5556")
+        backend = context.socket(zmq.XPUB)
+        backend.bind(self.pub_addr)
+        # backend.bind("tcp://*:5557")
+        try:
+            zmq.proxy(frontend, backend)
+        except KeyboardInterrupt:
+            pass
+
+        frontend.close()
+        backend.close()
+        context.term()
+        
+    def start(self):
+        if self.thd is None:
+            log.i("mq proxy starting...")
+            self.thd=threading.Thread(target=self._run, name="proxy")
+            self.thd.setDaemon(True)
+            self.thd.start()
+
+    def stop(self):
+        pass

pibot_ros/pypibot/transport/dataholder.py

@@ -0,0 +1,240 @@
+import struct
+
+params_size=29
+
+# main board
+class MessageID:
+    ID_GET_VERSION = 0
+    ID_SET_ROBOT_PARAMETER = 1
+    ID_GET_ROBOT_PARAMETER = 2
+    ID_INIT_ODOM = 3
+    ID_SET_VEL = 4
+    ID_GET_ODOM = 5
+    ID_GET_PID_DEBUG = 6
+    ID_GET_IMU = 7
+    ID_GET_ENCODER_COUNT = 8
+    ID_SET_MOTOR_PWM = 9
+
+class RobotMessage:
+    def pack(self):
+        return b''
+
+    def unpack(self):
+        return True
+
+class RobotFirmwareInfo(RobotMessage):
+    def __init__(self):
+        self.version = ''
+        self.build_time = ''
+
+    def unpack(self, data):
+        try:
+            upk = struct.unpack('16s16s', bytes(data))
+        except:
+            return False
+        [self.version, self.build_time] = upk
+        return True
+
+class RobotImuType:
+    IMU_TYPE_GY65 = 49
+    IMU_TYPE_GY85 = 69
+    IMU_TYPE_GY87 = 71
+
+class RobotModelType:
+    MODEL_TYPE_2WD_DIFF = 1
+    MODEL_TYPE_4WD_DIFF = 2
+    MODEL_TYPE_3WD_OMNI = 101
+    MODEL_TYPE_4WD_OMNI = 102
+    MODEL_TYPE_4WD_MECANUM = 201
+
+class RobotParameters():
+    def __init__(self, wheel_diameter=0, \
+                wheel_track=0, \
+                encoder_resolution=0, \
+                do_pid_interval=0, \
+                kp=0, \
+                ki=0, \
+                kd=0, \
+                ko=0, \
+                cmd_last_time=0, \
+                max_v_liner_x=0, \
+                max_v_liner_y=0, \
+                max_v_angular_z=0, \
+                imu_type=0, \
+                motor_ratio=0, \
+                model_type=0, \
+                motor_nonexchange_flag=255, \
+                encoder_nonexchange_flag=255, \
+                ):
+        self.wheel_diameter = wheel_diameter
+        self.wheel_track = wheel_track
+        self.encoder_resolution = encoder_resolution
+        self.do_pid_interval = do_pid_interval
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.ko = ko
+        self.cmd_last_time = cmd_last_time
+        self.max_v_liner_x = max_v_liner_x
+        self.max_v_liner_y = max_v_liner_y
+        self.max_v_angular_z = max_v_angular_z
+        self.imu_type = imu_type
+        self.motor_ratio = motor_ratio
+        self.model_type = model_type
+        self.motor_nonexchange_flag = motor_nonexchange_flag
+        self.encoder_nonexchange_flag = encoder_nonexchange_flag
+        reserve=b'\xff'
+        self.reserve=b''
+        for i in range(64-params_size):
+            self.reserve+=reserve
+robotParam = RobotParameters()
+
+class GetRobotParameters(RobotMessage):
+    def __init__(self):
+        self.param = robotParam
+
+    def unpack(self, data):
+        #print(bytes(data), len(bytes(data)))
+        upk = struct.unpack('<3H1B8H1B1H3B%ds'%(64-params_size), bytes(data))
+
+        [self.param.wheel_diameter,
+         self.param.wheel_track,
+         self.param.encoder_resolution,
+         self.param.do_pid_interval,
+         self.param.kp,
+         self.param.ki,
+         self.param.kd,
+         self.param.ko,
+         self.param.cmd_last_time,
+         self.param.max_v_liner_x,
+         self.param.max_v_liner_y,
+         self.param.max_v_angular_z,
+         self.param.imu_type,
+         self.param.motor_ratio,
+         self.param.model_type,
+         self.param.motor_nonexchange_flag,
+         self.param.encoder_nonexchange_flag,
+         self.param.reserve] = upk
+        return True
+
+class SetRobotParameters(RobotMessage):
+    def __init__(self):
+        self.param = robotParam
+
+    def pack(self):
+        data = [self.param.wheel_diameter,
+                self.param.wheel_track,
+                self.param.encoder_resolution,
+                self.param.do_pid_interval,
+                self.param.kp,
+                self.param.ki,
+                self.param.kd,
+                self.param.ko,
+                self.param.cmd_last_time,
+                self.param.max_v_liner_x,
+                self.param.max_v_liner_y,
+                self.param.max_v_angular_z,
+                self.param.imu_type,
+                self.param.motor_ratio,
+                self.param.model_type,
+                self.param.motor_nonexchange_flag,
+                self.param.encoder_nonexchange_flag,
+                self.param.reserve]
+
+        print(data)
+        pk = struct.pack('<3H1B8H1B1H3B%ds'%(64-(3*2+1+8*2+1+2+3)), *data)
+        return pk
+
+    def unpack(self, data):
+        return True
+
+class RobotVel(RobotMessage):
+    def __init__(self):
+        self.v_liner_x = 0
+        self.v_liner_y = 0
+        self.v_angular_z = 0
+
+    def pack(self):
+        data = [self.v_liner_x,
+                self.v_liner_y,
+                self.v_angular_z]
+        pk = struct.pack('3h', *data)
+        return pk
+
+    def unpack(self, data):
+        return True
+
+#todo the rest of the message classes
+class RobotOdom(RobotMessage):
+    def __init__(self):
+        self.v_liner_x = 0
+        self.v_liner_y = 0
+        self.v_angular_z = 0
+        self.x = 0
+        self.y = 0
+        self.yaw = 0
+
+    def unpack(self, data):
+        try:
+            upk = struct.unpack('<3H2l1H', bytes(data))
+        except:
+            return False
+        [self.v_liner_x,
+                self.v_liner_y,
+                self.v_angular_z,
+                self.x,
+                self.y,
+                self.yaw] = upk
+        return True
+
+class RobotPIDData(RobotMessage):
+    pass
+
+class RobotIMU(RobotMessage):
+    def __init__(self):
+        self.imu = [0]*9
+
+    def unpack(self, data):
+        try:
+            upk = struct.unpack('<9f', bytes(data))
+        except:
+            return False
+        
+        self.imu = upk
+        return True
+
+class RobotEncoderCount(RobotMessage):
+    def __init__(self):
+        self.encoder = [0]*4
+
+    def unpack(self, data):
+        try:
+            upk = struct.unpack('<4f', bytes(data))
+        except:
+            return False
+        
+        self.encoder = upk
+        return True
+
+class RobotMotorPWM(RobotMessage):
+    def __init__(self):
+        self.pwm = [0]*4
+
+    def pack(self):
+        pk = struct.pack('4h', *self.pwm)
+        return pk
+
+    def unpack(self, data):
+        return True
+
+BoardDataDict = {MessageID.ID_GET_VERSION:RobotFirmwareInfo(),
+            MessageID.ID_GET_ROBOT_PARAMETER:GetRobotParameters(),
+            MessageID.ID_SET_ROBOT_PARAMETER:SetRobotParameters(),
+            MessageID.ID_SET_VEL:RobotVel(),
+            MessageID.ID_GET_ODOM:RobotOdom(),
+            MessageID.ID_GET_PID_DEBUG: RobotPIDData(),
+            MessageID.ID_GET_IMU: RobotIMU(),
+            MessageID.ID_GET_ENCODER_COUNT: RobotEncoderCount(),
+            MessageID.ID_SET_MOTOR_PWM: RobotMotorPWM(),
+            }
+

pibot_ros/pypibot/transport/main.py

@@ -0,0 +1,115 @@
+import platform
+import sys
+sys.path.append("..")
+import pypibot
+from pypibot import log
+from transport import Transport
+from dataholder import MessageID
+import params
+import time
+import signal 
+
+#for linux
+port="/dev/pibot"
+
+#for windows
+#port="com3"
+
+pypibot.assistant.enableGlobalExcept()
+#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
+log.setLevel("i")
+
+run_flag = True
+
+def exit(signum, frame):
+    global run_flag
+    run_flag = False
+
+if __name__ == '__main__':
+    signal.signal(signal.SIGINT, exit)
+
+    mboard = Transport(port, params.pibotBaud)
+    if not mboard.start():
+        log.error("can not open %s"%port)
+        sys.exit()
+    
+    DataHolder = mboard.getDataHolder()
+
+    for num in range(0,3):
+        log.info("****************get robot version*****************")
+        boardVersion = DataHolder[MessageID.ID_GET_VERSION]
+        p = mboard.request(MessageID.ID_GET_VERSION)
+        if p:
+            log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
+            break
+        else:
+            log.error('read firmware version err\r\n')
+            import time
+            time.sleep(1)
+            if num == 2:
+                log.error('please check connection or baudrate\r\n')
+                sys.exit()
+                
+    # get robot parameter
+    robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
+    p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
+    if p:
+        log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
+                 %(robotParam.param.model_type, \
+                   robotParam.param.wheel_diameter, \
+                   robotParam.param.wheel_track, \
+                   robotParam.param.encoder_resolution
+                   ))
+
+        log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
+                 %(robotParam.param.do_pid_interval, \
+                   robotParam.param.kp, \
+                   robotParam.param.ki, \
+                   robotParam.param.kd, \
+                   robotParam.param.ko))
+
+        log.info("cmd_last_time:%d imu_type:%d" \
+                 %(robotParam.param.cmd_last_time,\
+                   robotParam.param.imu_type
+                   ))
+
+        log.info("max_v:%d %d %d\r\n" \
+                 %(robotParam.param.max_v_liner_x,\
+                   robotParam.param.max_v_liner_y, \
+                   robotParam.param.max_v_angular_z
+                   ))
+                   
+        log.info("motor flag:%d encoder flag: %d\r\n" \
+                 %(robotParam.param.motor_nonexchange_flag,\
+                   robotParam.param.encoder_nonexchange_flag
+                   ))
+    else:
+        log.error('get params err\r\n')
+        quit(1)
+
+    log.info("****************get odom&imu*****************")
+    while run_flag:
+        robotOdom = DataHolder[MessageID.ID_GET_ODOM]
+        p = mboard.request(MessageID.ID_GET_ODOM)
+        if p:
+            log.info('request get odom success, vx=%d vy=%d vangular=%d,    x=%d y=%d yaw=%d'%(robotOdom.v_liner_x, \
+                                                        robotOdom.v_liner_y, \
+                                                        robotOdom.v_angular_z, \
+                                                        robotOdom.x, \
+                                                        robotOdom.y, \
+                                                        robotOdom.yaw))
+        else:
+            log.error('get odom err')
+            quit(1)
+        
+        robotIMU = DataHolder[MessageID.ID_GET_IMU].imu
+        p = mboard.request(MessageID.ID_GET_IMU)
+        if p:
+            log.info('get imu success, imu=[%f %f %f    %f %f %f    %f %f %f]'%(robotIMU[0], robotIMU[1], robotIMU[2], \
+                                                                                        robotIMU[3], robotIMU[4], robotIMU[5], \
+                                                                                        robotIMU[6], robotIMU[7], robotIMU[8]))
+        else:
+            log.error('get imu err')
+            quit(1)
+       
+        time.sleep(0.1) 

pibot_ros/pypibot/transport/params.py

@@ -0,0 +1,75 @@
+import dataholder
+import os
+from dataholder import RobotImuType
+from dataholder import RobotModelType
+
+pibotModel = os.environ['PIBOT_MODEL']
+boardType = os.environ['PIBOT_BOARD']
+pibotBaud = os.environ['PIBOT_DRIVER_BAUDRATE']
+
+print(pibotModel)
+print(boardType)
+print(pibotBaud)
+
+pibotParam = dataholder.RobotParameters()
+
+if pibotModel == "apollo" and boardType == "arduino":
+    pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
+                                                75, 2500, 0, 10, \
+                                                250, 40, 0, 200, \
+                                                RobotImuType.IMU_TYPE_GY85, 90, \
+                                                RobotModelType.MODEL_TYPE_2WD_DIFF)
+elif pibotModel == "apollo" and boardType == "stm32f1":
+    pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
+                                                320, 2700, 0, 10, \
+                                                250, 50, 0, 200, \
+                                                RobotImuType.IMU_TYPE_GY87, 90, \
+                                                RobotModelType.MODEL_TYPE_2WD_DIFF)
+elif pibotModel == "apollo" and boardType == "stm32f4":
+    pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
+                                                320, 2700, 0, 10, \
+                                                250, 40, 0, 200, \
+                                                RobotImuType.IMU_TYPE_GY87, 90, \
+                                                RobotModelType.MODEL_TYPE_2WD_DIFF)
+elif pibotModel == "zeus" and boardType == "stm32f4":
+    pibotParam = dataholder.RobotParameters(58, 230, 44, 10, \
+                                                320, 2700, 0, 10, \
+                                                250, 50, 50, 250, \
+                                                RobotImuType.IMU_TYPE_GY87, 90, \
+                                                RobotModelType.MODEL_TYPE_3WD_OMNI)
+elif pibotModel == "hades" and boardType == "stm32f4":
+    pibotParam = dataholder.RobotParameters(76, 470, 44, 10, \
+                                                320, 2700, 0, 10, \
+                                                250, 50, 50, 250, \
+                                                RobotImuType.IMU_TYPE_GY87, 90, \
+                                                RobotModelType.MODEL_TYPE_4WD_MECANUM)
+elif pibotModel == "hadesX" and boardType == "stm32f4":
+    pibotParam = dataholder.RobotParameters(150, 565, 44, 10, \
+                                                250, 2750, 0, 10, \
+                                                250, 50, 50, 250, \
+                                                RobotImuType.IMU_TYPE_GY87, 72, \
+                                                RobotModelType.MODEL_TYPE_4WD_MECANUM)
+elif pibotModel == "hera" and boardType == "stm32f4":
+    pibotParam = dataholder.RobotParameters(82, 338, 44, 10, \
+                                                320, 2700, 0, 10, \
+                                                250, 50, 50, 250, \
+                                                RobotImuType.IMU_TYPE_GY87, 90, \
+                                                RobotModelType.MODEL_TYPE_4WD_DIFF)
+elif pibotModel == "apolloX" and boardType == "arduino":
+    pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
+                                                75, 2500, 0, 10, \
+                                                250, 40, 0, 200, \
+                                                RobotImuType.IMU_TYPE_GY85, 90, \
+                                                RobotModelType.MODEL_TYPE_2WD_DIFF)
+elif pibotModel == "apolloX" and boardType == "stm32f1":
+    pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
+                                                250, 1200, 0, 10, \
+                                                250, 50, 0, 200, \
+                                                RobotImuType.IMU_TYPE_GY87, 90, \
+                                                RobotModelType.MODEL_TYPE_2WD_DIFF)
+elif pibotModel == "apolloX" and boardType == "stm32f4":
+    pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
+                                                250, 1200, 0, 10, \
+                                                250, 50, 0, 200, \
+                                                RobotImuType.IMU_TYPE_GY87, 90, \
+                                                RobotModelType.MODEL_TYPE_2WD_DIFF)

pibot_ros/pypibot/transport/set_default_params.py

@@ -0,0 +1,90 @@
+import platform
+import sys
+sys.path.append("..")
+import pypibot
+from pypibot import log
+from transport import Transport
+from dataholder import MessageID
+import params
+
+#for linux
+port="/dev/pibot"
+
+#for windows
+#port="com3"
+
+pypibot.assistant.enableGlobalExcept()
+#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
+log.setLevel("i")
+
+if __name__ == '__main__':
+    mboard = Transport(port, params.pibotBaud)
+    if not mboard.start():
+        log.error("can not open %s"%port)
+        sys.exit()
+    
+    DataHolder = mboard.getDataHolder()
+
+    for num in range(0,3):
+        log.info("****************get robot version*****************")
+        boardVersion = DataHolder[MessageID.ID_GET_VERSION]
+        p = mboard.request(MessageID.ID_GET_VERSION)
+        if p:
+            log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
+            break
+        else:
+            log.error('read firmware version err\r\n')
+            import time
+            time.sleep(1)
+            if num == 2:
+                log.error('please check connection or baudrate\r\n')
+                sys.exit()
+                
+    # set robot parameter
+    log.info("****************set robot parameter*****************")
+    
+    DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = params.pibotParam
+
+    p = mboard.request(MessageID.ID_SET_ROBOT_PARAMETER)
+    if p:
+        log.info('set parameter success')
+    else:
+        log.error('set parameter err')
+        quit(1)
+
+    # get robot parameter
+    robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
+    p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
+    if p:
+        log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
+                 %(robotParam.param.model_type, \
+                   robotParam.param.wheel_diameter, \
+                   robotParam.param.wheel_track, \
+                   robotParam.param.encoder_resolution
+                   ))
+
+        log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
+                 %(robotParam.param.do_pid_interval, \
+                   robotParam.param.kp, \
+                   robotParam.param.ki, \
+                   robotParam.param.kd, \
+                   robotParam.param.ko))
+
+        log.info("cmd_last_time:%d imu_type:%d" \
+                 %(robotParam.param.cmd_last_time,\
+                   robotParam.param.imu_type
+                   ))
+
+        log.info("max_v:%d %d %d\r\n" \
+                 %(robotParam.param.max_v_liner_x,\
+                   robotParam.param.max_v_liner_y, \
+                   robotParam.param.max_v_angular_z
+                   ))
+                   
+        log.info("motor flag:%d encoder flag: %d\r\n" \
+                 %(robotParam.param.motor_nonexchange_flag,\
+                   robotParam.param.encoder_nonexchange_flag
+                   ))
+    else:
+        log.error('get param err\r\n')
+        quit(1)

pibot_ros/pypibot/transport/test_motors.py

@@ -0,0 +1,122 @@
+import platform
+import sys
+sys.path.append("..")
+import pypibot
+from pypibot import log
+from transport import Transport
+from dataholder import MessageID
+import params
+import signal 
+
+#for linux
+port="/dev/pibot"
+
+#for windows
+#port="com3"
+
+pypibot.assistant.enableGlobalExcept()
+#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
+log.setLevel("i")
+
+run_flag = True
+
+def exit(signum, frame):
+    global run_flag
+    run_flag = False
+
+if __name__ == '__main__':
+    signal.signal(signal.SIGINT, exit)
+
+    mboard = Transport(port, params.pibotBaud)
+    if not mboard.start():
+        log.error("can not open %s"%port)
+        sys.exit()
+    
+    pwm=[0]*4
+    for i in range(len(sys.argv)-1):
+        pwm[i] = int(sys.argv[i+1])
+    
+    DataHolder = mboard.getDataHolder()
+
+    for num in range(0,3):
+        log.info("****************get robot version*****************")
+        boardVersion = DataHolder[MessageID.ID_GET_VERSION]
+        p = mboard.request(MessageID.ID_GET_VERSION)
+        if p:
+            log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
+            break
+        else:
+            log.error('read firmware version err\r\n')
+            import time
+            time.sleep(1)
+            if num == 2:
+                log.error('please check connection or baudrate\r\n')
+                sys.exit()
+                
+    # get robot parameter
+    robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
+    p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
+    if p:
+        log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
+                 %(robotParam.param.model_type, \
+                   robotParam.param.wheel_diameter, \
+                   robotParam.param.wheel_track, \
+                   robotParam.param.encoder_resolution
+                   ))
+
+        log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
+                 %(robotParam.param.do_pid_interval, \
+                   robotParam.param.kp, \
+                   robotParam.param.ki, \
+                   robotParam.param.kd, \
+                   robotParam.param.ko))
+
+        log.info("cmd_last_time:%d imu_type:%d" \
+                 %(robotParam.param.cmd_last_time,\
+                   robotParam.param.imu_type
+                   ))
+
+        log.info("max_v:%d %d %d\r\n" \
+                 %(robotParam.param.max_v_liner_x,\
+                   robotParam.param.max_v_liner_y, \
+                   robotParam.param.max_v_angular_z
+                   ))
+                   
+        log.info("motor flag:%d encoder flag: %d\r\n" \
+                 %(robotParam.param.motor_nonexchange_flag,\
+                   robotParam.param.encoder_nonexchange_flag
+                   ))
+    else:
+        log.error('get params err\r\n')
+        quit(1)
+
+    DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = pwm
+
+    p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
+    if p:
+        log.info('set pwm success')
+    else:
+        log.error('set pwm err')
+        quit(1)
+
+    log.info("****************get encoder count*****************")
+    while run_flag:
+        robotEncoder = DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder
+        p = mboard.request(MessageID.ID_GET_ENCODER_COUNT)
+        if p:
+            log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robotEncoder])))
+        else:
+            log.error('get encoder count err')
+            quit(1)
+
+        import time
+        time.sleep(0.5)
+
+    DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4
+
+    p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
+    if p:
+        log.info('set pwm success')
+    else:
+        log.error('set pwm err')
+        quit(1)

pibot_ros/pypibot/transport/transport.py

@@ -0,0 +1,197 @@
+import sys
+sys.path.append("..")
+import pypibot
+from pypibot import log
+from pypibot import assistant
+
+import serial
+import threading
+import struct
+import time
+from dataholder import MessageID, BoardDataDict
+FIX_HEAD = 0x5a
+
+class Recstate():
+    WAITING_HD = 0
+    WAITING_MSG_ID = 1
+    RECEIVE_LEN = 2
+    RECEIVE_PACKAGE = 3
+    RECEIVE_CHECK = 4
+
+def checksum(d):
+    sum = 0
+    if assistant.is_python3():
+        for i in d:
+            sum += i
+            sum = sum&0xff
+    else:
+        for i in d:
+            sum += ord(i)
+            sum = sum&0xff
+    return sum
+
+
+class Transport:
+    def __init__(self, port, baudrate=921600):
+        self._Port = port
+        self._Baudrate = baudrate
+        self._KeepRunning = False
+        self.receive_state = Recstate.WAITING_HD
+        self.rev_msg = []
+        self.rev_data = []
+        self.wait_event = threading.Event()
+
+    def getDataHolder(self):
+        return BoardDataDict
+
+    def start(self):
+        try:
+            self._Serial = serial.Serial(port=self._Port, baudrate=self._Baudrate, timeout=0.2)
+            self._KeepRunning = True
+            self._ReceiverThread = threading.Thread(target=self._Listen)
+            self._ReceiverThread.setDaemon(True)
+            self._ReceiverThread.start()
+            return True
+        except:
+            return False
+
+    def Stop(self):
+        self._KeepRunning = False
+        time.sleep(0.1)
+        self._Serial.close()
+
+    def _Listen(self):
+        while self._KeepRunning:
+            if self.receiveFiniteStates(self._Serial.read()):
+                self.packageAnalysis()
+
+    def receiveFiniteStates(self, s):
+        if len(s) == 0:
+            return  False
+        val = s[0]
+        val_int = val
+        if not assistant.is_python3():
+            val_int = ord(val)
+
+        if self.receive_state == Recstate.WAITING_HD:
+            if val_int == FIX_HEAD:
+                log.trace('got head')
+                self.rev_msg = []
+                self.rev_data =[]
+                self.rev_msg.append(val)
+                self.receive_state = Recstate.WAITING_MSG_ID
+        elif self.receive_state == Recstate.WAITING_MSG_ID:
+            log.trace('got msg id')
+            self.rev_msg.append(val)
+            self.receive_state = Recstate.RECEIVE_LEN
+        elif self.receive_state == Recstate.RECEIVE_LEN:
+            log.trace('got len:%d', val_int)
+            self.rev_msg.append(val)
+            if val_int == 0:
+                self.receive_state = Recstate.RECEIVE_CHECK
+            else:
+                self.receive_state = Recstate.RECEIVE_PACKAGE
+        elif self.receive_state == Recstate.RECEIVE_PACKAGE:
+            # if assistant.is_python3(): 
+            #     self.rev_data.append((chr(val)).encode('latin1'))
+            # else:
+            self.rev_data.append(val)
+            r = False
+            if assistant.is_python3(): 
+                r = len(self.rev_data) == int(self.rev_msg[-1])
+            else:
+                r = len(self.rev_data) == ord(self.rev_msg[-1])
+            
+            if r:
+                self.rev_msg.extend(self.rev_data)
+                self.receive_state = Recstate.RECEIVE_CHECK
+        elif self.receive_state == Recstate.RECEIVE_CHECK:
+            log.trace('got check')
+            self.receive_state = Recstate.WAITING_HD
+            if val_int == checksum(self.rev_msg):
+                log.trace('got a complete message')
+                return True
+        else:
+            self.receive_state = Recstate.WAITING_HD
+
+        # continue receiving
+        return False
+
+    def packageAnalysis(self):
+        if assistant.is_python3(): 
+            in_msg_id = int(self.rev_msg[1])
+        else:
+            in_msg_id = ord(self.rev_msg[1])
+        if assistant.is_python3(): 
+            log.debug("recv body:" + " ".join("{:02x}".format(c) for c in self.rev_data))
+            r = BoardDataDict[in_msg_id].unpack(bytes(self.rev_data))
+        else:
+            log.debug("recv body:" + " ".join("{:02x}".format(ord(c)) for c in self.rev_data))
+            r = BoardDataDict[in_msg_id].unpack(''.join(self.rev_data))
+        if r:
+            self.res_id = in_msg_id
+            if in_msg_id<100:
+                self.set_response()
+            else:#notify
+                log.debug('msg %d'%self.rev_msg[4],'data incoming')
+                pass
+        else:
+            log.debug ('error unpacking pkg')
+
+    def request(self, id, timeout=0.5):
+        if not self.write(id):
+            log.debug ('Serial send error!')
+            return False
+        if self.wait_for_response(timeout):
+            if id == self.res_id:
+                log.trace ('OK')
+            else:
+                log.error ('Got unmatched response!')
+        else:
+            log.error ('Request got no response!')
+            return False
+        # clear response
+        self.res_id = None
+        return True
+
+    def write(self, id):
+        cmd = self.make_command(id)
+        if assistant.is_python3():
+            log.d("write:" + " ".join("{:02x}".format(c) for c in cmd))
+        else:
+            log.d("write:" + " ".join("{:02x}".format(ord(c)) for c in cmd))
+        self._Serial.write(cmd)
+        return True
+
+    def wait_for_response(self, timeout):
+        self.wait_event.clear()
+        return self.wait_event.wait(timeout)
+
+    def set_response(self):
+        self.wait_event.set()
+
+    def make_command(self, id):
+        #print(DataDict[id])
+        data = BoardDataDict[id].pack()
+        l = [FIX_HEAD, id, len(data)]
+        head = struct.pack("3B", *l)
+        body = head + data
+        
+        if assistant.is_python3():
+            return body + chr(checksum(body)).encode('latin1')
+        else:
+            return body + chr(checksum(body))
+
+
+if __name__ == '__main__':
+
+    mboard = Transport('com10')
+    if not mboard.start():
+        import sys
+        sys.exit()
+
+    p = mboard.request(MessageID.ID_GET_VERSION)
+    log.i("result=%s"%p)
+    print('Version =[',mboard.getDataHolder()[MessageID.ID_GET_VERSION].version.decode(), mboard.getDataHolder()[MessageID.ID_GET_VERSION].build_time.decode(),"]\r\n")
+
+

pibot_ros/ros_ws/.catkin_workspace

@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration

pibot_ros/ros_ws/src/.gitignore

@@ -0,0 +1 @@
+ros_package

pibot_ros/ros_ws/src/arbotix_ros/.gitignore

@@ -0,0 +1,5 @@
+*.pyc
+.project
+.cproject
+.pydevproject
+.vscode

pibot_ros/ros_ws/src/arbotix_ros/LICENSE

@@ -0,0 +1,25 @@
+Copyright (c) 2008-2011 Vanadium Labs LLC. 
+All right reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+  * Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+  * Redistributions in binary form must reproduce the above copyright
+    notice, this list of conditions and the following disclaimer in the
+    documentation and/or other materials provided with the distribution.
+  * Neither the name of Vanadium Labs LLC nor the names of its 
+    contributors may be used to endorse or promote products derived 
+    from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

pibot_ros/ros_ws/src/arbotix_ros/README.md

@@ -0,0 +1,3 @@
+## Arbotix Drivers
+
+This repository contains the Arbotix ROS drivers, catkinized, and ready for ROS Noetic.