Skip to content

Commit

Permalink
Merge pull request #35 from umdlife/feat/stream-publisher
Browse files Browse the repository at this point in the history 
Enable encoded and decoded stream publishing
  • Loading branch information
@vicmassy
vicmassy authored Jan 22, 2024
2 parents c9629d5 + 8527797 commit 0fdac3f
Show file tree
Hide file tree
Showing 4 changed files with 109 additions and 57 deletions.
1 change: 1 addition & 0 deletions psdk_interfaces/srv/CameraSetupStreaming.srv
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
uint8 payload_index 1 # please refer to enum E_DjiLiveViewCameraPosition in dji_liveview.h
uint8 camera_source 0 # please refer to enum E_DjiLiveViewCameraSource in dji_liveview.h
bool start_stop # 0 for stop and 1 for start
bool decoded_output 1 # 0 for encoded output and 1 for decoded output
---
#response
bool success
25 changes: 23 additions & 2 deletions psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1611,6 +1611,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
* @param request CameraSetupStreaming service request. The camera
* mounted position for which the request is made needs to be specified as
* well as the camera source (e.g. using the wide or the zoom camera).
* Moreover, the user can choose to stream the images raw or decoded.
* @param response CameraSetupStreaming service response.
*/
void camera_setup_streaming_cb(
Expand Down Expand Up @@ -1903,7 +1904,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
void set_local_altitude_reference(const float altitude);

/**
* @brief Starts the main camera streaming.
* @brief Starts the camera streaming.
* @param callback function to be executed when a frame is received
* @param user_data unused parameter
* @param payload_index select which camera to use to retrieve the streaming.
Expand All @@ -1914,7 +1915,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
* @return true/false Returns true if the streaming has been started
* correctly and False otherwise.
*/
bool start_main_camera_stream(CameraImageCallback callback, void* user_data,
bool start_camera_stream(CameraImageCallback callback, void* user_data,
const E_DjiLiveViewCameraPosition payload_index,
const E_DjiLiveViewCameraSource camera_source);
/**
Expand All @@ -1935,12 +1936,31 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
* @param user_data unused parameter
*/
void publish_main_camera_images(CameraRGBImage rgb_img, void* user_data);

/**
* @brief Publishes the raw (not decoded) main camera streaming to a ROS 2
* topic
* @param buffer raw buffer retrieved from the camera
* @param buffer_length length of the buffer
*/
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
* @param user_data unused parameter
*/
void publish_fpv_camera_images(CameraRGBImage rgb_img, void* user_data);

/**
* @brief Publishes the raw (not decoded) FPV camera streaming to a ROS 2
* topic
* @param buffer raw buffer retrieved from the camera
* @param buffer_length length of the buffer
*/
void publish_fpv_camera_images(const uint8_t* buffer, uint32_t buffer_length);

/**
* @brief Get the optical frame id for a certain lens
* @return string with the optical frame id name
Expand Down Expand Up @@ -1969,6 +1989,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
E_DjiCameraType attached_camera_type_;
E_DjiLiveViewCameraSource selected_camera_source_;
bool publish_camera_transforms_{false};
bool decode_stream_{true};
};

/**
Expand Down
132 changes: 81 additions & 51 deletions psdk_wrapper/src/modules/liveview.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ PSDKWrapper::init_liveview()
{DJI_LIVEVIEW_CAMERA_POSITION_NO_2, (new DJICameraStreamDecoder())},
{DJI_LIVEVIEW_CAMERA_POSITION_NO_3, (new DJICameraStreamDecoder())},
};
decode_stream_ = true;
return true;
}

Expand All @@ -58,8 +59,12 @@ c_LiveviewConvertH264ToRgbCallback(E_DjiLiveViewCameraPosition position,
const uint8_t *buffer,
uint32_t buffer_length)
{
return global_ptr_->LiveviewConvertH264ToRgbCallback(position, buffer,
buffer_length);
if (global_ptr_->decode_stream_)
{
return global_ptr_->LiveviewConvertH264ToRgbCallback(position, buffer,
buffer_length);
}
return global_ptr_->publish_main_camera_images(buffer, buffer_length);
}

void
Expand Down Expand Up @@ -95,10 +100,12 @@ PSDKWrapper::camera_setup_streaming_cb(
static_cast<E_DjiLiveViewCameraPosition>(request->payload_index);
selected_camera_source_ =
static_cast<E_DjiLiveViewCameraSource>(request->camera_source);
RCLCPP_INFO(
get_logger(),
"Setting up camera streaming for payload index %d and camera source %d",
payload_index, selected_camera_source_);
decode_stream_ = request->decoded_output;

RCLCPP_INFO(get_logger(),
"Setting up camera streaming for payload index %d and camera "
"source %d. Output decoded: %d",
payload_index, selected_camera_source_);

if (request->start_stop)
{
Expand All @@ -107,15 +114,15 @@ PSDKWrapper::camera_setup_streaming_cb(
if (payload_index == DJI_LIVEVIEW_CAMERA_POSITION_NO_1)
{
char main_camera_name[] = "MAIN_CAMERA";
streaming_result = start_main_camera_stream(
&c_publish_main_streaming_callback, &main_camera_name, payload_index,
streaming_result = start_camera_stream(&c_publish_main_streaming_callback,
&main_camera_name, payload_index,
selected_camera_source_);
}
else if (payload_index == DJI_LIVEVIEW_CAMERA_POSITION_FPV)
{
char fpv_camera_name[] = "FPV_CAMERA";
streaming_result = start_main_camera_stream(
&c_publish_fpv_streaming_callback, &fpv_camera_name, payload_index,
streaming_result = start_camera_stream(&c_publish_fpv_streaming_callback,
&fpv_camera_name, payload_index,
selected_camera_source_);
}

Expand Down Expand Up @@ -147,40 +154,39 @@ PSDKWrapper::camera_setup_streaming_cb(
}

bool
PSDKWrapper::start_main_camera_stream(CameraImageCallback callback,
void *user_data,
PSDKWrapper::start_camera_stream(CameraImageCallback callback, void *user_data,
E_DjiLiveViewCameraPosition payload_index,
E_DjiLiveViewCameraSource camera_source)
{
auto decoder = stream_decoder.find(payload_index);

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

T_DjiReturnCode return_code = DjiLiveview_StartH264Stream(
payload_index, camera_source, c_LiveviewConvertH264ToRgbCallback);

if (return_code != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
auto decoder = stream_decoder.find(payload_index);
if ((decoder != stream_decoder.end()) && decoder->second)
{
RCLCPP_ERROR(get_logger(),
"Failed to start camera streaming, error code: %ld.",
return_code);
return false;
decoder->second->init();
decoder->second->registerCallback(callback, user_data);
}
else
{
RCLCPP_INFO(get_logger(), "Successfully started the camera streaming.");
return true;
RCLCPP_ERROR(get_logger(), "Failed to set-up the decoder");
return false;
}
}
else

T_DjiReturnCode return_code = DjiLiveview_StartH264Stream(
payload_index, camera_source, c_LiveviewConvertH264ToRgbCallback);
if (return_code != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
RCLCPP_ERROR(get_logger(), "Failed to decode streaming, error code: %ld",
DJI_ERROR_SYSTEM_MODULE_CODE_NOT_FOUND);
RCLCPP_ERROR(get_logger(),
"Failed to start camera streaming, error code: %ld.",
return_code);
return false;
}
else
{
RCLCPP_INFO(get_logger(), "Successfully started the camera streaming.");
return true;
}
}

bool
Expand Down Expand Up @@ -209,35 +215,59 @@ PSDKWrapper::stop_main_camera_stream(
}
}

void
PSDKWrapper::publish_main_camera_images(const uint8_t *buffer,
uint32_t buffer_length)
{
auto img = std::make_unique<sensor_msgs::msg::Image>();
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(std::move(img));
}

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

void
PSDKWrapper::publish_main_camera_images(CameraRGBImage rgb_img, void *user_data)
{
(void)user_data;
sensor_msgs::msg::Image img;
img.height = rgb_img.height;
img.width = rgb_img.width;
img.step = rgb_img.width * 3;
img.encoding = "rgb8";
img.data = rgb_img.rawData;

img.header.stamp = this->get_clock()->now();
img.header.frame_id = get_optical_frame_id();
main_camera_stream_pub_->publish(img);
auto img = std::make_unique<sensor_msgs::msg::Image>();
img->height = rgb_img.height;
img->width = rgb_img.width;
img->step = rgb_img.width * 3;
img->encoding = "rgb8";
img->data = rgb_img.rawData;

img->header.stamp = this->get_clock()->now();
img->header.frame_id = get_optical_frame_id();
main_camera_stream_pub_->publish(std::move(img));
}

void
PSDKWrapper::publish_fpv_camera_images(CameraRGBImage rgb_img, void *user_data)
{
(void)user_data;
sensor_msgs::msg::Image img;
img.height = rgb_img.height;
img.width = rgb_img.width;
img.step = rgb_img.width * 3;
img.encoding = "rgb8";
img.data = rgb_img.rawData;

img.header.stamp = this->get_clock()->now();
img.header.frame_id = "fpv_camera_link";
fpv_camera_stream_pub_->publish(img);
auto img = std::make_unique<sensor_msgs::msg::Image>();
img->height = rgb_img.height;
img->width = rgb_img.width;
img->step = rgb_img.width * 3;
img->encoding = "rgb8";
img->data = rgb_img.rawData;

img->header.stamp = this->get_clock()->now();
img->header.frame_id = "fpv_camera_link";
fpv_camera_stream_pub_->publish(std::move(img));
}
} // namespace psdk_ros2
8 changes: 4 additions & 4 deletions psdk_wrapper/src/psdk_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,9 +21,9 @@ using namespace std::placeholders; // NOLINT

namespace psdk_ros2
{

PSDKWrapper::PSDKWrapper(const std::string &node_name)
: rclcpp_lifecycle::LifecycleNode(node_name, "", rclcpp::NodeOptions())
: rclcpp_lifecycle::LifecycleNode(
node_name, "", rclcpp::NodeOptions().use_intra_process_comms(true))
{
RCLCPP_INFO(get_logger(), "Creating Constructor PSDKWrapper");
declare_parameter("app_name", rclcpp::ParameterValue(""));
Expand Down Expand Up @@ -820,9 +820,9 @@ PSDKWrapper::initialize_ros_elements()
create_publisher<psdk_interfaces::msg::RelativeObstacleInfo>(
"psdk_ros2/relative_obstacle_info", 10);
main_camera_stream_pub_ = create_publisher<sensor_msgs::msg::Image>(
"psdk_ros2/main_camera_stream", 10);
"psdk_ros2/main_camera_stream", rclcpp::SensorDataQoS());
fpv_camera_stream_pub_ = create_publisher<sensor_msgs::msg::Image>(
"psdk_ros2/fpv_camera_stream", 10);
"psdk_ros2/fpv_camera_stream", rclcpp::SensorDataQoS());
control_mode_pub_ = create_publisher<psdk_interfaces::msg::ControlMode>(
"psdk_ros2/control_mode", 10);
home_point_pub_ =
Expand Down

0 comments on commit 0fdac3f

Please sign in to comment.