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

Enable encoded and decoded stream publishing #35

Merged
merged 8 commits into from
Jan 22, 2024
Merged
Changes from 1 commit
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
Prev Previous commit
Next Next commit
Publish enconded images
vicmassy committed Nov 22, 2023
commit 9e1bf707af577db6dd59849bd60729e74012a48b
2 changes: 2 additions & 0 deletions psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp
Original file line number Diff line number Diff line change
@@ -1862,6 +1862,8 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
* @param user_data unused parameter
*/
void publish_main_camera_images(CameraRGBImage rgb_img, void* user_data);
void publish_main_camera_images(const uint8_t *buffer, uint32_t buffer_length);

/**
* @brief Publishes the FPV camera streaming to a ROS 2 topic
* @param rgb_img decoded RGB frame retrieved from the camera
42 changes: 28 additions & 14 deletions psdk_wrapper/src/modules/liveview.cpp
Original file line number Diff line number Diff line change
@@ -58,8 +58,10 @@ c_LiveviewConvertH264ToRgbCallback(E_DjiLiveViewCameraPosition position,
const uint8_t *buffer,
uint32_t buffer_length)
{
return global_ptr_->LiveviewConvertH264ToRgbCallback(position, buffer,
buffer_length);
// return global_ptr_->LiveviewConvertH264ToRgbCallback(position, buffer,
// buffer_length);

return global_ptr_->publish_main_camera_images(buffer, buffer_length);
}

void
@@ -72,6 +74,7 @@ PSDKWrapper::LiveviewConvertH264ToRgbCallback(
{
decoder->second->decodeBuffer(buffer, buffer_length);
}

}

void
@@ -152,12 +155,12 @@ PSDKWrapper::start_main_camera_stream(CameraImageCallback callback,
E_DjiLiveViewCameraPosition payload_index,
E_DjiLiveViewCameraSource camera_source)
{
auto decoder = stream_decoder.find(payload_index);
// auto decoder = stream_decoder.find(payload_index);

if ((decoder != stream_decoder.end()) && decoder->second)
{
decoder->second->init();
decoder->second->registerCallback(callback, user_data);
// if ((decoder != stream_decoder.end()) && decoder->second)
// {
// decoder->second->init();
// decoder->second->registerCallback(callback, user_data);

T_DjiReturnCode return_code = DjiLiveview_StartH264Stream(
payload_index, camera_source, c_LiveviewConvertH264ToRgbCallback);
@@ -174,13 +177,13 @@ PSDKWrapper::start_main_camera_stream(CameraImageCallback callback,
RCLCPP_INFO(get_logger(), "Successfully started the camera streaming.");
return true;
}
}
else
{
RCLCPP_ERROR(get_logger(), "Failed to decode streaming, error code: %ld",
DJI_ERROR_SYSTEM_MODULE_CODE_NOT_FOUND);
return false;
}
// }
// else
// {
// RCLCPP_ERROR(get_logger(), "Failed to decode streaming, error code: %ld",
// DJI_ERROR_SYSTEM_MODULE_CODE_NOT_FOUND);
// return false;
// }
}

bool
@@ -209,6 +212,17 @@ PSDKWrapper::stop_main_camera_stream(
}
}

void
PSDKWrapper::publish_main_camera_images(const uint8_t *buffer, uint32_t buffer_length)
{
sensor_msgs::msg::Image img;
img.encoding = "h264";
img.data = std::vector<uint8_t>(buffer, buffer+buffer_length);
img.header.stamp = this->get_clock()->now();
img.header.frame_id = get_optical_frame_id();
main_camera_stream_pub_->publish(img);
}

void
PSDKWrapper::publish_main_camera_images(CameraRGBImage rgb_img, void *user_data)
{
2 changes: 1 addition & 1 deletion psdk_wrapper/src/psdk_wrapper.cpp
Original file line number Diff line number Diff line change
@@ -787,7 +787,7 @@ PSDKWrapper::initialize_ros_elements()
create_publisher<geometry_msgs::msg::AccelStamped>(
"psdk_ros2/acceleration_body_raw", 10);
main_camera_stream_pub_ = create_publisher<sensor_msgs::msg::Image>(
"psdk_ros2/main_camera_stream", 1);
"psdk_ros2/main_camera_stream", rclcpp::SensorDataQoS());
fpv_camera_stream_pub_ = create_publisher<sensor_msgs::msg::Image>(
"psdk_ros2/fpv_camera_stream", 10);
vicmassy marked this conversation as resolved.
Show resolved Hide resolved
control_mode_pub_ = create_publisher<psdk_interfaces::msg::ControlMode>(