From a72b671779a6f3db053b160fc56d60d2e3971093 Mon Sep 17 00:00:00 2001 From: Yamboy1 Date: Fri, 9 Feb 2024 14:08:31 +1300 Subject: [PATCH 1/5] Remove unnecessary whitespace --- psdk_wrapper/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/psdk_wrapper/CMakeLists.txt b/psdk_wrapper/CMakeLists.txt index 586a7546..6204cb9b 100644 --- a/psdk_wrapper/CMakeLists.txt +++ b/psdk_wrapper/CMakeLists.txt @@ -34,7 +34,7 @@ find_package(std_srvs REQUIRED) # Add path to FindFFMPEG.cmake and FindLIBUSB.cmake files set(PSDK_PATH ${PROJECT_SOURCE_DIR}/../../Payload-SDK) -set(CMAKE_MODULE_PATH +set(CMAKE_MODULE_PATH ${PSDK_PATH}/samples/sample_c++/platform/linux/common/3rdparty) find_package(FFMPEG REQUIRED) @@ -67,7 +67,7 @@ if (PSDK_LIB) message(STATUS "PSDK library found: ${PSDK_LIB}") else() message(FATAL_ERROR "PSDK library NOT found") -endif() +endif() if (FFMPEG_FOUND) message(STATUS "Found FFMPEG installed in the system") From 26d49b400eba143d560ab06d733a0e589198dfae Mon Sep 17 00:00:00 2001 From: Yamboy1 Date: Fri, 9 Feb 2024 14:09:07 +1300 Subject: [PATCH 2/5] Add vscode folder to gitignore --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index c39c1b98..992bca71 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ docs/doxyoutput docs/api -.pre-commit-config.yaml \ No newline at end of file +.pre-commit-config.yaml +.vscode/ From afd4ff1061fb8b29cd269ceab7b6588d7175925f Mon Sep 17 00:00:00 2001 From: Yamboy1 Date: Fri, 9 Feb 2024 14:10:16 +1300 Subject: [PATCH 3/5] Add low speed transmission module --- psdk_interfaces/CMakeLists.txt | 1 + psdk_interfaces/msg/TransmissionData.msg | 2 + psdk_wrapper/CMakeLists.txt | 1 + .../include/psdk_wrapper/psdk_wrapper.hpp | 25 +++++++++ .../src/modules/data_transmission.cpp | 56 +++++++++++++++++++ psdk_wrapper/src/psdk_wrapper.cpp | 5 +- 6 files changed, 89 insertions(+), 1 deletion(-) create mode 100644 psdk_interfaces/msg/TransmissionData.msg create mode 100644 psdk_wrapper/src/modules/data_transmission.cpp diff --git a/psdk_interfaces/CMakeLists.txt b/psdk_interfaces/CMakeLists.txt index 7223390f..e4d4302a 100644 --- a/psdk_interfaces/CMakeLists.txt +++ b/psdk_interfaces/CMakeLists.txt @@ -36,6 +36,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/FlightStatus.msg" "msg/RCConnectionStatus.msg" "msg/ControlMode.msg" + "msg/TransmissionData.msg" "srv/CameraGetType.srv" "srv/CameraSetExposureModeEV.srv" "srv/CameraGetExposureModeEV.srv" diff --git a/psdk_interfaces/msg/TransmissionData.msg b/psdk_interfaces/msg/TransmissionData.msg new file mode 100644 index 00000000..44c23373 --- /dev/null +++ b/psdk_interfaces/msg/TransmissionData.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +string data diff --git a/psdk_wrapper/CMakeLists.txt b/psdk_wrapper/CMakeLists.txt index 6204cb9b..fd73d704 100644 --- a/psdk_wrapper/CMakeLists.txt +++ b/psdk_wrapper/CMakeLists.txt @@ -140,6 +140,7 @@ add_library(psdk_wrapper_libs SHARED src/modules/camera.cpp src/modules/gimbal.cpp src/modules/liveview.cpp + src/modules/data_transmission.cpp ${DJI_OSAL_PATH}/osal_fs.c ${DJI_OSAL_PATH}/osal_socket.c ${DJI_OSAL_PATH}/osal.c diff --git a/psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp b/psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp index b66efb0a..a24583b3 100644 --- a/psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp +++ b/psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -76,6 +77,7 @@ #include "psdk_interfaces/msg/rc_connection_status.hpp" #include "psdk_interfaces/msg/relative_obstacle_info.hpp" #include "psdk_interfaces/msg/rtk_yaw.hpp" +#include "psdk_interfaces/msg/transmission_data.hpp" #include "psdk_interfaces/srv/camera_delete_file_by_index.hpp" #include "psdk_interfaces/srv/camera_download_file_by_index.hpp" #include "psdk_interfaces/srv/camera_download_file_list.hpp" @@ -334,6 +336,16 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode * @return true/false */ bool deinit_liveview(); + /** + * @brief Initialize the data transmisison module + * @return true/false + */ + bool init_data_transmission(); + /** + * @brief Deinitialize the data transmisison module + * @return true/false + */ + bool deinit_data_transmission(); /** * @brief Get the DJI frequency object associated with a certain frequency @@ -500,6 +512,9 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode E_DjiLiveViewCameraPosition position, const uint8_t* buffer, uint32_t buffer_length); + friend T_DjiReturnCode c_ReceiveDataFromMobileCallback(const uint8_t* data, + uint16_t len); + /*C++ type DJI topic subscriber callbacks*/ /** * @brief Retrieves the attitude quaternion provided by DJI PSDK lib and @@ -1751,6 +1766,8 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode main_camera_stream_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr fpv_camera_stream_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + low_speed_transmission_data_pub_; /* ROS 2 Subscribers */ rclcpp::Subscription::SharedPtr @@ -1942,6 +1959,14 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode * @param user_data unused parameter */ void publish_fpv_camera_images(CameraRGBImage rgb_img, void* user_data); + /** + * @brief Publishes the raw (not decoded) data received over the + * LowSpeedDataChannel from the RC MSDK app. + * @param data raw data retrieved from the RC + * @param len length of the data + */ + T_DjiReturnCode publish_data_from_mobile(const uint8_t* data, uint16_t len); + /** * @brief Get the optical frame id for a certain lens * @return string with the optical frame id name diff --git a/psdk_wrapper/src/modules/data_transmission.cpp b/psdk_wrapper/src/modules/data_transmission.cpp new file mode 100644 index 00000000..bb28cf54 --- /dev/null +++ b/psdk_wrapper/src/modules/data_transmission.cpp @@ -0,0 +1,56 @@ +#include +#include "psdk_wrapper/psdk_wrapper.hpp" + +E_DjiChannelAddress channelAddress = DJI_CHANNEL_ADDRESS_MASTER_RC_APP; + +namespace psdk_ros2 +{ + +T_DjiReturnCode c_ReceiveDataFromMobileCallback(const uint8_t* data, uint16_t len) { + return global_ptr_->publish_data_from_mobile(data, len); +} +bool +PSDKWrapper::init_data_transmission() +{ + RCLCPP_INFO(get_logger(), "Initiating data_transmission module..."); + if (DjiLowSpeedDataChannel_Init() != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { + RCLCPP_ERROR(get_logger(), "Could not initialize data_transmission module."); + return false; + } + + T_DjiReturnCode return_code = DjiLowSpeedDataChannel_RegRecvDataCallback(channelAddress, + c_ReceiveDataFromMobileCallback); + if (return_code != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { + RCLCPP_ERROR(get_logger(), "Could not initialize receive data callback."); + return false; + } + return true; +} + +bool PSDKWrapper::deinit_data_transmission() +{ + RCLCPP_INFO(get_logger(), "Deinitializing data_transmission module..."); + if (DjiLowSpeedDataChannel_DeInit() != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { + RCLCPP_ERROR(get_logger(), "Could not deinitialize the data_transmission module."); + return false; + } + return true; +} + + + +T_DjiReturnCode PSDKWrapper::publish_data_from_mobile(const uint8_t* data, + uint16_t len) +{ + auto trans_data = std::make_unique(); + // The +1 appears to be neccessary, as otherwise there is an additional byte before the + // data starts, that appears to be the length of the data. + std::string string_data { (char*) (data+1), len-1 }; + trans_data->data = string_data; + trans_data->header.stamp = this->get_clock()->now(); + trans_data->header.frame_id = get_optical_frame_id(); + low_speed_transmission_data_pub_->publish(std::move(trans_data)); + return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS; +} + +} diff --git a/psdk_wrapper/src/psdk_wrapper.cpp b/psdk_wrapper/src/psdk_wrapper.cpp index 71ec7e1a..ce1624bb 100644 --- a/psdk_wrapper/src/psdk_wrapper.cpp +++ b/psdk_wrapper/src/psdk_wrapper.cpp @@ -83,7 +83,7 @@ PSDKWrapper::on_configure(const rclcpp_lifecycle::State &state) return CallbackReturn::FAILURE; } if (!init_telemetry() || !init_flight_control() || !init_camera_manager() || - !init_gimbal_manager() || !init_liveview()) + !init_gimbal_manager() || !init_liveview() || !init_data_transmission()) { return CallbackReturn::FAILURE; } @@ -834,6 +834,8 @@ PSDKWrapper::initialize_ros_elements() "psdk_ros2/altitude_sea_level", 10); altitude_barometric_pub_ = create_publisher( "psdk_ros2/altitude_barometric", 10); + low_speed_transmission_data_pub_ = create_publisher( + "psdk_ros2/low_speed_transmission_data", 10); RCLCPP_INFO(get_logger(), "Creating subscribers"); flight_control_generic_sub_ = create_subscription( @@ -1131,6 +1133,7 @@ PSDKWrapper::activate_ros_elements() home_point_altitude_pub_->on_activate(); altitude_sl_pub_->on_activate(); altitude_barometric_pub_->on_activate(); + low_speed_transmission_data_pub_->on_activate(); } void From 71eab00f961a890cb32317ee941eb0e6fcc8c68b Mon Sep 17 00:00:00 2001 From: Yamboy1 Date: Fri, 16 Feb 2024 11:14:23 +1300 Subject: [PATCH 4/5] Add mandatory module checks --- psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp | 1 + psdk_wrapper/src/psdk_wrapper.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp b/psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp index f9e12ebe..e37be08e 100644 --- a/psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp +++ b/psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp @@ -2052,6 +2052,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode bool is_gimbal_module_mandatory_{true}; bool is_flight_control_module_mandatory_{true}; bool is_liveview_module_mandatory_{true}; + bool is_data_transmission_module_mandatory_{true}; }; /** diff --git a/psdk_wrapper/src/psdk_wrapper.cpp b/psdk_wrapper/src/psdk_wrapper.cpp index b91921ff..1d4a8404 100644 --- a/psdk_wrapper/src/psdk_wrapper.cpp +++ b/psdk_wrapper/src/psdk_wrapper.cpp @@ -43,6 +43,7 @@ PSDKWrapper::PSDKWrapper(const std::string &node_name) declare_parameter("mandatory_modules.camera", rclcpp::ParameterValue(true)); declare_parameter("mandatory_modules.gimbal", rclcpp::ParameterValue(true)); declare_parameter("mandatory_modules.liveview", rclcpp::ParameterValue(true)); + declare_parameter("mandatory_modules.data_transmission", rclcpp::ParameterValue(true)); declare_parameter("imu_frame", rclcpp::ParameterValue("psdk_imu_link")); declare_parameter("body_frame", rclcpp::ParameterValue("psdk_base_link")); declare_parameter("map_frame", rclcpp::ParameterValue("psdk_map_enu")); @@ -372,6 +373,7 @@ PSDKWrapper::load_parameters() get_parameter("mandatory_modules.camera", is_camera_module_mandatory_); get_parameter("mandatory_modules.gimbal", is_gimbal_module_mandatory_); get_parameter("mandatory_modules.liveview", is_liveview_module_mandatory_); + get_parameter("mandatory_modules.data_transmission", is_data_transmission_module_mandatory_); if (!get_parameter("imu_frame", params_.imu_frame)) { From 49f96ff959e7e06131dee8e43a3005eb0797532d Mon Sep 17 00:00:00 2001 From: Yamboy1 Date: Mon, 26 Feb 2024 16:19:35 +1300 Subject: [PATCH 5/5] Update mandatory modules --- docs/documentation/GettingStarted.md | 17 +++++++++-------- psdk_wrapper/cfg/psdk_params.yaml | 5 +++-- psdk_wrapper/src/psdk_wrapper.cpp | 2 +- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/docs/documentation/GettingStarted.md b/docs/documentation/GettingStarted.md index 29dbcc63..9c4269bd 100644 --- a/docs/documentation/GettingStarted.md +++ b/docs/documentation/GettingStarted.md @@ -1,6 +1,6 @@ # Getting started -To use the psdk_ros2 wrapper you will need to create a new workspace in which you clone both the wrapper as well as the Payload-SDK libraries. +To use the psdk_ros2 wrapper you will need to create a new workspace in which you clone both the wrapper as well as the Payload-SDK libraries. ```bash mkdir -p ~/psdk_ros2_ws/src @@ -14,7 +14,7 @@ rosdep update rosdep keys --from-paths . --ignore-src --rosdistro humble | \ xargs rosdep resolve --rosdistro humble | \ awk '/#apt/{getline; print}' > ./rosdep_requirements.txt -sudo apt install -y --no-install-recommends $(cat ./rosdep_requirements.txt) +sudo apt install -y --no-install-recommends $(cat ./rosdep_requirements.txt) # Build the code cd ~/psdk_ros2_ws @@ -56,6 +56,7 @@ The following parameters can be configured in the *psdk_wrapper/cfg/psdk_params. | - camera | Bool | True | Trigger node failure, if module not loaded | | - gimbal | Bool | True | Trigger node failure, if module not loaded | | - liveview | Bool | True | Trigger node failure, if module not loaded | +| - data_transmission | Bool | True | Trigger node failure, if module not loaded | | data_frequency | Object | - | Options are: 1, 5, 10, 50, 100, 200, 400 Hz | | - imu | Integer | 100 | - | | - attitude | Integer | 100 | - | @@ -74,7 +75,7 @@ The following parameters can be configured in the *psdk_wrapper/cfg/psdk_params. | - control_information | Integer | 1 | - | -To configure the hardware connection type and to specify the exact ports that need to be used, please use the *psdk_wrapper/cfg/link_config.json* file. This file follows a similar strategy to the file one must configure before running the DJI PSDK samples. Thus, for simplicity, the psdk_ros2 wrapper follows the same approach. Please notice, that the App configuration (e.g. app_id, app_key) has been kept in the ros parameter file. +To configure the hardware connection type and to specify the exact ports that need to be used, please use the *psdk_wrapper/cfg/link_config.json* file. This file follows a similar strategy to the file one must configure before running the DJI PSDK samples. Thus, for simplicity, the psdk_ros2 wrapper follows the same approach. Please notice, that the App configuration (e.g. app_id, app_key) has been kept in the ros parameter file. ## Udev rules @@ -89,7 +90,7 @@ SUBSYSTEM=="tty", SUBSYSTEMS=="usb", ATTRS{idVendor}=="YourVendor", ATTRS{idProd -## Dependencies +## Dependencies ### ROS 2 packages @@ -110,9 +111,9 @@ The following ROS 2 packages are needed to successfully build the wrapper: The following libraries are needded to enable the access to USB devices and handling the video streaming: * libusb-1.0-0-dev -* libopus-dev -* ffmpeg -* libavcodec-dev -* libavformat-dev +* libopus-dev +* ffmpeg +* libavcodec-dev +* libavformat-dev * libavfilter-dev diff --git a/psdk_wrapper/cfg/psdk_params.yaml b/psdk_wrapper/cfg/psdk_params.yaml index cf7cf7b3..a361960d 100644 --- a/psdk_wrapper/cfg/psdk_params.yaml +++ b/psdk_wrapper/cfg/psdk_params.yaml @@ -26,8 +26,9 @@ camera: false gimbal: false liveview: false - - data_frequency: # Options are: 1, 5, 10, 50, 100, 200, 400 Hz + data_transmission: true + + data_frequency: # Options are: 1, 5, 10, 50, 100, 200, 400 Hz imu: 100 attitude: 100 acceleration: 50 diff --git a/psdk_wrapper/src/psdk_wrapper.cpp b/psdk_wrapper/src/psdk_wrapper.cpp index 1d4a8404..f859cf83 100644 --- a/psdk_wrapper/src/psdk_wrapper.cpp +++ b/psdk_wrapper/src/psdk_wrapper.cpp @@ -1444,7 +1444,7 @@ PSDKWrapper::initialize_psdk_modules() {std::bind(&PSDKWrapper::init_gimbal_manager, this), is_gimbal_module_mandatory_}, {std::bind(&PSDKWrapper::init_liveview, this), - is_liveview_module_mandatory_} + is_liveview_module_mandatory_}, {std::bind(&PSDKWrapper::init_data_transmission, this), is_data_transmission_module_mandatory_}};