Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feat/low speed data transmission #58

Open
wants to merge 7 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -2,4 +2,5 @@ docs/doxyoutput
docs/api
docs/_build/doctrees

.pre-commit-config.yaml
.pre-commit-config.yaml
.vscode/
21 changes: 10 additions & 11 deletions docs/documentation/GettingStarted.md
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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
Expand Down Expand Up @@ -57,6 +57,7 @@ The following parameters can be configured in the *psdk_wrapper/cfg/psdk_params.
| - gimbal | Bool | False | Trigger node failure, if module not loaded |
| - liveview | Bool | False | Trigger node failure, if module not loaded |
| - hms | Bool | False | 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 | - |
Expand All @@ -75,7 +76,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 (cfg/psdk_params.yml).
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

Expand All @@ -90,7 +91,7 @@ SUBSYSTEM=="tty", SUBSYSTEMS=="usb", ATTRS{idVendor}=="YourVendor", ATTRS{idProd



## Dependencies
## Dependencies

### ROS 2 packages

Expand All @@ -108,14 +109,12 @@ The following ROS 2 packages are needed to successfully build the wrapper:

### Other libraries

The following libraries are needed to enable the access to USB devices and handling the video streaming:
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

The following library is used to work with JSON:
* nlohmann-json-dev
1 change: 1 addition & 0 deletions psdk_interfaces/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/FlightStatus.msg"
"msg/RCConnectionStatus.msg"
"msg/ControlMode.msg"
"msg/TransmissionData.msg"
"msg/HmsInfoMsg.msg"
"msg/HmsInfoTable.msg"
"msg/SingleBatteryInfo.msg"
Expand Down
2 changes: 2 additions & 0 deletions psdk_interfaces/msg/TransmissionData.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
std_msgs/Header header
string data
5 changes: 3 additions & 2 deletions psdk_wrapper/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,7 @@ add_library(psdk_wrapper_libs SHARED
src/modules/camera.cpp
src/modules/gimbal.cpp
src/modules/liveview.cpp
src/modules/data_transmission.cpp
src/modules/hms.cpp
${DJI_OSAL_PATH}/osal_fs.c
${DJI_OSAL_PATH}/osal_socket.c
Expand Down Expand Up @@ -203,7 +204,7 @@ install(TARGETS
RUNTIME DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY
install(DIRECTORY
include/
${PSDK_PATH}/psdk_lib/include/
${DJI_STREAMING_PATH}/
Expand All @@ -212,7 +213,7 @@ install(DIRECTORY
${DJI_UTILS_PATH}/
${DJI_CONFIG_MANAGER_PATH}/
DESTINATION include/
FILES_MATCHING
FILES_MATCHING
PATTERN "*.h"
PATTERN "*.hpp"
)
Expand Down
1 change: 1 addition & 0 deletions psdk_wrapper/cfg/psdk_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
gimbal: false
liveview: false
hms: false
data_transmission: true

data_frequency: # Options are: 1, 5, 10, 50, 100, 200, 400 Hz
imu: 100
Expand Down
26 changes: 26 additions & 0 deletions psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include <dji_logger.h>
#include <dji_platform.h>
#include <dji_typedef.h>
#include <dji_low_speed_data_channel.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>

Expand Down Expand Up @@ -79,6 +80,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/msg/single_battery_info.hpp"
#include "psdk_interfaces/srv/camera_delete_file_by_index.hpp"
#include "psdk_interfaces/srv/camera_download_file_by_index.hpp"
Expand Down Expand Up @@ -337,6 +339,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 Initialize the health monitoring system (HMS) module
* @note Since the HMS module callback function involves a ROS2
Expand Down Expand Up @@ -523,6 +535,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
Expand Down Expand Up @@ -1813,6 +1828,8 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
main_camera_stream_pub_;
rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Image>::SharedPtr
fpv_camera_stream_pub_;
rclcpp_lifecycle::LifecyclePublisher<psdk_interfaces::msg::TransmissionData>::SharedPtr
low_speed_transmission_data_pub_;

/* ROS 2 Subscribers */
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr
Expand Down Expand Up @@ -2023,6 +2040,14 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
*/
void publish_fpv_camera_images(const uint8_t* buffer, uint32_t buffer_length);

/**
* @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
Expand Down Expand Up @@ -2106,6 +2131,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
bool is_flight_control_module_mandatory_{true};
bool is_liveview_module_mandatory_{true};
bool is_hms_module_mandatory_{true};
bool is_data_transmission_module_mandatory_{true};
};

/**
Expand Down
56 changes: 56 additions & 0 deletions psdk_wrapper/src/modules/data_transmission.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,56 @@
#include <string>
#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<psdk_interfaces::msg::TransmissionData>();
// 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;
}

}
13 changes: 10 additions & 3 deletions psdk_wrapper/src/psdk_wrapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"));
Expand Down Expand Up @@ -166,7 +167,7 @@ PSDKWrapper::on_shutdown(const rclcpp_lifecycle::State &state)
// Deinitialize all remaining modules
if (!deinit_telemetry() || !deinit_flight_control() ||
!deinit_camera_manager() || !deinit_gimbal_manager() ||
!deinit_liveview() || !deinit_hms())
!deinit_liveview() || !deinit_hms() || !deinit_data_transmission())
{
return CallbackReturn::FAILURE;
}
Expand Down Expand Up @@ -380,6 +381,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_);
get_parameter("mandatory_modules.hms", is_hms_module_mandatory_);

if (!get_parameter("imu_frame", params_.imu_frame))
Expand Down Expand Up @@ -488,7 +490,7 @@ PSDKWrapper::load_parameters()
params_.velocity_frequency = VELOCITY_TOPICS_MAX_FREQ;
}

if (!get_parameter("data_frequency.angular_velocity",
if (!get_parameter("data_frequency.angular_velocity",
params_.angular_rate_frequency))
{
RCLCPP_ERROR(get_logger(), "angular_velocity param not defined");
Expand Down Expand Up @@ -875,6 +877,8 @@ PSDKWrapper::initialize_ros_elements()
"psdk_ros2/altitude_sea_level", 10);
altitude_barometric_pub_ = create_publisher<std_msgs::msg::Float32>(
"psdk_ros2/altitude_barometric", 10);
low_speed_transmission_data_pub_ = create_publisher<psdk_interfaces::msg::TransmissionData>(
"psdk_ros2/low_speed_transmission_data", 10);
hms_info_table_pub_ = create_publisher<psdk_interfaces::msg::HmsInfoTable>(
"psdk_ros2/hms_info_table", 10);

Expand Down Expand Up @@ -1176,6 +1180,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();
hms_info_table_pub_->on_activate();
}

Expand Down Expand Up @@ -1470,7 +1475,9 @@ 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_}};

for (const auto &initializer : module_initializers)
{
Expand Down
Loading