Multi image encoding and isaac_ros_tensor_rt & YOLOv5-with-Isaac-ROS

Autonomous Machines Robotics - Isaac Isaac ROS
,
1

Hello all,

I am currently writing an image encoder in cpp that synchronizes 6 incoming camera streams and sends them to the TensorRT server from the DNN Inference GIT Hub repository (topic name: tensor_pub, GitHub - NVIDIA-ISAAC-ROS/isaac_ros_dnn_inference: Hardware-accelerated DNN model inference ROS 2 packages using NVIDIA Triton/TensorRT for both Jetson and x86_64 with CUDA-capable GPU). The DNN model was re-exported to TensorRT format and configured with a batch size of 6. The configuration obtained works fine with the test publisher (isaac_ros_dnn_inference_test) which is included and a dimension definition of [6 x 3 x 640 x 640] (image size of Yolov5s). As soon as I try to use my encoder instead of the test version, the TensorRT node starts but cannot use the data and terminates.

Since I have very little experience with the Isaac ROS package or C++, I’m sure the code below needs improvement. However, if there are fundamentally better approaches to using multiple images at once with the existing encoder I am also grateful for this advice.

My system is built as follows:
Ubuntu 20.04
RTX 3080 Ti
ROS 2 Humble (Within docker container)

Error message from console:

[component_container_mt-1] [INFO] [1688738782.932835963] [yolov5_container]: Load Library: /workspaces/isaac_ros-dev/install/isaac_ros_tensor_rt/lib/libtensor_rt_node.so
[component_container_mt-1] [INFO] [1688738782.939710727] [yolov5_container]: Found class: rclcpp_components::NodeFactoryTemplatenvidia::isaac_ros::dnn_inference::TensorRTNode
[component_container_mt-1] [INFO] [1688738782.939758629] [yolov5_container]: Instantiate class: rclcpp_components::NodeFactoryTemplatenvidia::isaac_ros::dnn_inference::TensorRTNode
[component_container_mt-1] [INFO] [1688738782.943790852] [NitrosContext]: [NitrosContext] Creating a new shared context
[component_container_mt-1] [INFO] [1688738782.943974922] [tensor_rt_node]: [NitrosNode] Initializing NitrosNode
[component_container_mt-1] [INFO] [1688738782.944599355] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/std/libgxf_std.so
[component_container_mt-1] [INFO] [1688738782.947413155] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_gxf_helpers.so
[component_container_mt-1] [INFO] [1688738782.949189612] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_sight.so
[component_container_mt-1] [INFO] [1688738782.950909167] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_atlas.so
[component_container_mt-1] [INFO] [1688738782.953283301] [NitrosContext]: [NitrosContext] Loading application: ‘/workspaces/isaac_ros-dev/install/isaac_ros_nitros/share/isaac_ros_nitros/config/type_adapter_nitros_context_graph.yaml’
[component_container_mt-1] [INFO] [1688738782.953695947] [NitrosContext]: [NitrosContext] Initializing application

[component_container_mt-1] [INFO] [1688738782.954876117] [NitrosContext]: [NitrosContext] Running application

[component_container_mt-1] 2023-07-07 16:06:22.954 WARN gxf/std/program.cpp@456: No system specified. Nothing to do
[component_container_mt-1] [INFO] [1688738782.956106549] [tensor_rt_node]: [TensorRTNode] Set input data format to: “nitros_tensor_list_nchw_rgb_f32”
[component_container_mt-1] [INFO] [1688738782.956133406] [tensor_rt_node]: [TensorRTNode] Set output data format to: “nitros_tensor_list_nhwc_rgb_f32”
[component_container_mt-1] [INFO] [1688738782.956190211] [tensor_rt_node]: [NitrosNode] Starting NitrosNode
[component_container_mt-1] [INFO] [1688738782.956196634] [tensor_rt_node]: [NitrosNode] Loading built-in preset extension specs
[component_container_mt-1] [INFO] [1688738782.957536372] [tensor_rt_node]: [NitrosNode] Loading built-in extension specs
[component_container_mt-1] [INFO] [1688738782.957568226] [tensor_rt_node]: [NitrosNode] Loading preset extension specs
[component_container_mt-1] [INFO] [1688738782.957878123] [tensor_rt_node]: [NitrosNode] Loading extension specs
[component_container_mt-1] [INFO] [1688738782.957891064] [tensor_rt_node]: [NitrosNode] Loading generator rules
[component_container_mt-1] [INFO] [1688738782.957989796] [tensor_rt_node]: [NitrosNode] Loading extensions
[component_container_mt-1] [INFO] [1688738782.958117015] [tensor_rt_node]: [NitrosContext] Loading extension: gxf/lib/libgxf_message_compositor.so
[component_container_mt-1] [INFO] [1688738782.958469509] [tensor_rt_node]: [NitrosContext] Loading extension: gxf/lib/cuda/libgxf_cuda.so
[component_container_mt-1] [INFO] [1688738782.960064913] [tensor_rt_node]: [NitrosContext] Loading extension: gxf/lib/serialization/libgxf_serialization.so
[component_container_mt-1] [INFO] [1688738782.961810917] [tensor_rt_node]: [NitrosContext] Loading extension: gxf/tensor_rt/libgxf_tensor_rt.so
[component_container_mt-1] [INFO] [1688738782.985373886] [tensor_rt_node]: [NitrosNode] Loading graph to the optimizer
[component_container_mt-1] [INFO] [1688738782.986289000] [tensor_rt_node]: [NitrosNode] Running optimization
[component_container_mt-1] [INFO] [1688738782.993096912] [tensor_rt_node]: [NitrosNode] Obtaining graph IO group info from the optimizer
[component_container_mt-1] [INFO] [1688738782.993662083] [tensor_rt_node]: [NitrosNode] Creating negotiated publishers/subscribers
[component_container_mt-1] [INFO] [1688738782.993703693] [tensor_rt_node]: [NitrosPublisherSubscriberGroup] Pinning the component “inference/rx” (type=“nvidia::gxf::DoubleBufferReceiver”) to use its compatible format only: “nitros_tensor_list_nchw_rgb_f32”
[component_container_mt-1] [INFO] [1688738782.993715228] [tensor_rt_node]: [NitrosPublisherSubscriberGroup] Pinning the component “sink/sink” (type=“nvidia::isaac_ros::MessageRelay”) to use its compatible format only: “nitros_tensor_list_nhwc_rgb_f32”
[component_container_mt-1] [INFO] [1688738782.996291939] [tensor_rt_node]: [NitrosNode] Starting negotiation

[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node ‘/tensor_rt_node’ in container ‘/yolov5_container’
[component_container_mt-1] [INFO] [1688738783.306182668] [tensor_rt_node]: [NitrosSubscriber] Received a message but the application receiver’s pointer is not yet set.
[component_container_mt-1] [INFO] [1688738783.327717883] [tensor_rt_node]: [NitrosSubscriber] Received a message but the application receiver’s pointer is not yet set.
[component_container_mt-1] [INFO] [1688738783.528248821] [tensor_rt_node]: [NitrosSubscriber] Received a message but the application receiver’s pointer is not yet set.
[component_container_mt-1] [INFO] [1688738783.591797151] [tensor_rt_node]: [NitrosSubscriber] Received a message but the application receiver’s pointer is not yet set.
[component_container_mt-1] [INFO] [1688738783.649925940] [tensor_rt_node]: [NitrosSubscriber] Received a message but the application receiver’s pointer is not yet set.
[component_container_mt-1] [INFO] [1688738783.693255055] [tensor_rt_node]: [NitrosSubscriber] Received a message but the application receiver’s pointer is not yet set.
[component_container_mt-1] [INFO] [1688738783.728475753] [tensor_rt_node]: [NitrosSubscriber] Received a message but the application receiver’s pointer is not yet set.
[component_container_mt-1] [INFO] [1688738783.974897945] [tensor_rt_node]: [NitrosSubscriber] Received a message but the application receiver’s pointer is not yet set.
[component_container_mt-1] [INFO] [1688738783.996632667] [tensor_rt_node]: [NitrosNode] Starting post negotiation setup
[component_container_mt-1] [INFO] [1688738783.996685737] [tensor_rt_node]: [NitrosNode] Getting data format negotiation results
[component_container_mt-1] [INFO] [1688738783.996705612] [tensor_rt_node]: [NitrosPublisher] Negotiation ended with no results
[component_container_mt-1] [INFO] [1688738783.996715697] [tensor_rt_node]: [NitrosPublisher] Use only the compatible publisher: topic_name=“/tensor_sub”, data_format=“nitros_tensor_list_nhwc_rgb_f32”
[component_container_mt-1] [INFO] [1688738783.996728216] [tensor_rt_node]: [NitrosSubscriber] Negotiation ended with no results
[component_container_mt-1] [INFO] [1688738783.996736542] [tensor_rt_node]: [NitrosSubscriber] Use the compatible subscriber: topic_name=“/tensor_pub”, data_format=“nitros_tensor_list_nchw_rgb_f32”
[component_container_mt-1] [INFO] [1688738783.996780194] [tensor_rt_node]: [NitrosNode] Exporting the final graph based on the negotiation results
[component_container_mt-1] [INFO] [1688738783.999305430] [tensor_rt_node]: [NitrosNode] Wrote the final top level YAML graph to “/workspaces/isaac_ros-dev/install/isaac_ros_tensor_rt/share/isaac_ros_tensor_rt/WCKNFFXDVF.yaml”
[component_container_mt-1] [INFO] [1688738783.999365316] [tensor_rt_node]: [NitrosNode] Calling user’s pre-load-graph callback
[component_container_mt-1] [INFO] [1688738783.999370498] [tensor_rt_node]: [NitrosNode] Loading application
[component_container_mt-1] [INFO] [1688738783.999377083] [tensor_rt_node]: [NitrosContext] Loading application: ‘/workspaces/isaac_ros-dev/install/isaac_ros_tensor_rt/share/isaac_ros_tensor_rt/WCKNFFXDVF.yaml’
[component_container_mt-1] 2023-07-07 16:06:24.000 WARN gxf/std/yaml_file_loader.cpp@952: Using unregistered parameter ‘dummy_rx’ in component ‘’.
[component_container_mt-1] [INFO] [1688738784.000318729] [tensor_rt_node]: [NitrosNode] Linking Nitros pub/sub to the loaded application
[component_container_mt-1] [INFO] [1688738784.000407688] [tensor_rt_node]: [NitrosNode] Calling user’s post-load-graph callback
[component_container_mt-1] [INFO] [1688738784.000417311] [tensor_rt_node]: In TensorRTNode postLoadGraphCallback().
[component_container_mt-1] [INFO] [1688738784.000466941] [tensor_rt_node]: [NitrosContext] Initializing application

[component_container_mt-1] [INFO] [1688738784.000906928] [tensor_rt_node]: [NitrosContext] Running application

[component_container_mt-1] [INFO] [1688738784.000969599] [tensor_rt_node]: [NitrosNode] Starting a heartbeat timer (eid=38)
[component_container_mt-1] 2023-07-07 16:06:25.193 WARN /workspaces/isaac_ros-dev/src/isaac_ros_dnn_inference/isaac_ros_tensor_rt/gxf/tensor_rt/tensor_rt_inference.cpp@152: TRT WARNING: CUDA lazy loading is not enabled. Enabling it can significantly reduce device memory usage. See CUDA_MODULE_LOADING in CUDA C++ Programming Guide
[component_container_mt-1] 2023-07-07 16:06:25.194 ERROR /workspaces/isaac_ros-dev/src/isaac_ros_dnn_inference/isaac_ros_tensor_rt/gxf/tensor_rt/tensor_rt_inference.cpp@569: Failed to retrieve Tensor input_tensor
[component_container_mt-1] 2023-07-07 16:06:25.194 ERROR gxf/std/entity_executor.cpp@509: Failed to tick codelet in entity: WCKNFFXDVF_inference code: GXF_FAILURE
[component_container_mt-1] 2023-07-07 16:06:25.194 ERROR gxf/std/entity_executor.cpp@540: Entity [WCKNFFXDVF_inference] must be in Lifecycle::kStarted or Lifecycle::kIdle stage before stopping. Current state is Ticking
[component_container_mt-1] 2023-07-07 16:06:25.194 ERROR gxf/std/entity_executor.cpp@540: Entity [WCKNFFXDVF_inference] must be in Lifecycle::kStarted or Lifecycle::kIdle stage before stopping. Current state is Ticking
[component_container_mt-1] 2023-07-07 16:06:25.194 WARN gxf/std/greedy_scheduler.cpp@241: Error while executing entity 38 named ‘WCKNFFXDVF_inference’: GXF_FAILURE
[component_container_mt-1] 2023-07-07 16:06:26.001 ERROR gxf/std/entity_executor.cpp@203: Entity with eid 38 not found!
[component_container_mt-1] [WARN] [1688738786.001085979] [tensor_rt_node]: [NitrosNode] The heartbeat entity (eid=38) was stopped. The graph may have been terminated.
[component_container_mt-1] [INFO] [1688738786.003692799] [tensor_rt_node]: [NitrosNode] Terminating the running application
[component_container_mt-1] [INFO] [1688738786.003709023] [tensor_rt_node]: [NitrosContext] Interrupting GXF

[component_container_mt-1] [INFO] [1688738786.003728824] [tensor_rt_node]: [NitrosContext] Waiting on GXF

[component_container_mt-1] 2023-07-07 16:06:26.003 ERROR gxf/std/program.cpp@497: wait failed. Deactivating

[component_container_mt-1] 2023-07-07 16:06:26.014 ERROR gxf/core/runtime.cpp@1251: Graph wait failed with error: GXF_FAILURE
[component_container_mt-1] [ERROR] [1688738786.014308999] [tensor_rt_node]: [NitrosContext] GxfGraphWait Error: GXF_FAILURE
[component_container_mt-1] [INFO] [1688738786.014316068] [tensor_rt_node]: [NitrosNode] Application termination done
[INFO] [component_container_mt-1]: process has finished cleanly [pid 177245]

Used source code:
cpp:

include “
/include/debug_encoding_node.hpp”

using namespace std::chrono_literals;

DebugEncodingNode::DebugEncodingNode() : Node(“debug_encoding_node”)
{
bridge_ = std::make_shared<cv_bridge::CvImage>();

w_ = 640;
h_ = 640;
c_ = 3;
n_ = 6;
SIZE_FLOAT_ = 4;
network_img_size_ = 640;

cam_img_sub_1_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, “/camera/color/image_raw_1”);
cam_img_sub_2_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, “/camera/color/image_raw_2”);
cam_img_sub_3_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, “/camera/color/image_raw_3”);
cam_img_sub_4_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, “/camera/color/image_raw_4”);
cam_img_sub_5_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, “/camera/color/image_raw_5”);
cam_img_sub_6_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, “/camera/color/image_raw_6”);

sync_ = std::make_shared<message_filters::Synchronizer>(SyncPolicy(10), *cam_img_sub_1_, *cam_img_sub_2_, *cam_img_sub_3_, *cam_img_sub_4_, *cam_img_sub_5_, *cam_img_sub_6_);
sync_->registerCallback(&DebugEncodingNode::tsCallback, this);

publisher_ = this->create_publisher<isaac_ros_tensor_list_interfaces::msg::TensorList>(“tensor_pub”, 10);

RCLCPP_INFO(this->get_logger(), " *** Launched encoding *** ");
}

void DebugEncodingNode::printTensorShape(
const isaac_ros_tensor_list_interfaces::msg::Tensor& tensor,
const rclcpp::Logger& logger)
{
RCLCPP_INFO(logger, “Tensor shape”);
RCLCPP_INFO(logger, “Rank: %d”, tensor.shape.rank);
for (const auto& dim : tensor.shape.dims) {
RCLCPP_INFO(logger, “Dimension: %d”, dim);
}
RCLCPP_INFO(logger, “Tensor data size: %zu”, tensor.data.size());
}

void DebugEncodingNode::tsCallback(const sensor_msgs::msg::Image::ConstSharedPtr img1,
const sensor_msgs::msg::Image::ConstSharedPtr img2,
const sensor_msgs::msg::Image::ConstSharedPtr img3,
const sensor_msgs::msg::Image::ConstSharedPtr img4,
const sensor_msgs::msg::Image::ConstSharedPtr img5,
const sensor_msgs::msg::Image::ConstSharedPtr img6)
{
auto tensor_list = isaac_ros_tensor_list_interfaces::msg::TensorList();
tensor_list.header = img1->header;

auto tensor_shape = isaac_ros_tensor_list_interfaces::msg::TensorShape();
tensor_shape.rank = 4; // uint8 Tensor Rank
tensor_shape.dims = {6, 3, 640, 640};

auto tensor = isaac_ros_tensor_list_interfaces::msg::Tensor();
tensor.name = “InputTensor”;
tensor.shape = tensor_shape;
tensor.data_type = 9; // int32: “9=float32”

tensor.strides = {
static_cast(n_ * c_ * h_ * w_ * SIZE_FLOAT_),
static_cast(n_ * h_ * w_ * SIZE_FLOAT_),
static_cast(n_ * w_ * SIZE_FLOAT_),
static_cast(n_ * SIZE_FLOAT_)
};

auto t_start = std::chrono::high_resolution_clock::now();

std::vectorcv::Mat cv2_imgs = {
cv_bridge::toCvCopy(img1, “bgr8”)->image,
cv_bridge::toCvCopy(img2, “bgr8”)->image,
cv_bridge::toCvCopy(img3, “bgr8”)->image,
cv_bridge::toCvCopy(img4, “bgr8”)->image,
cv_bridge::toCvCopy(img5, “bgr8”)->image,
cv_bridge::toCvCopy(img6, “bgr8”)->image
};

// Resize
std::vectorcv::Mat resized_data;
for (const auto& img : cv2_imgs)
{
cv::Mat resized_img = resizeWithPad(img, cv::Size(640, 640), cv::Scalar(0, 0, 0));
resized_data.push_back(resized_img);
}

// Normalize
std::vectorcv::Mat normalized_data;
for (const auto& element : resized_data)
{
cv::Mat normalized_img = transposeNormalize(element);
normalized_data.push_back(normalized_img);
}

// Tensor data
std::vectorcv::Mat tensor_data;
for (const auto& element : normalized_data)
{
cv::Mat tensor_img = element.clone();
tensor_img = tensor_img.reshape(1, 1);
tensor_data.push_back(tensor_img);
}

// Convert tensor_data into a vector of uint8_t
std::vector<uint32_t> tensor_data_vec;
for (const auto& tensor_img : tensor_data)
{
tensor_data_vec.insert(tensor_data_vec.end(), tensor_img.data, tensor_img.data + tensor_img.total());
}

tensor.data.resize(tensor_data_vec.size()*SIZE_FLOAT_);
memcpy(tensor.data.data(), tensor_data_vec.data(), tensor_data_vec.size()*SIZE_FLOAT_);

tensor_list.tensors = {tensor};

RCLCPP_INFO(this->get_logger(), “Timing: %fs”, std::chrono::duration(std::chrono::high_resolution_clock::now() - t_start).count());

publisher_->publish(tensor_list);

printTensorShape(tensor, this->get_logger());
}

cv::Mat DebugEncodingNode::resizeWithPad(const cv::Mat& image, const cv::Size& new_shape, const cv::Scalar& padding_color)
{
cv::Size original_shape = cv::Size(image.cols, image.rows);
float ratio = static_cast(std::max(new_shape.width, new_shape.height)) / std::max(original_shape.width, original_shape.height);
cv::Size new_size(static_cast(std::round(original_shape.width * ratio)), static_cast(std::round(original_shape.height * ratio)));
cv::Mat resized_img;
cv::resize(image, resized_img, new_size);

int delta_w = new_shape.width - new_size.width;
int delta_h = new_shape.height - new_size.height;
int top = delta_h / 2;
int bottom = delta_h - top;
int left = delta_w / 2;
int right = delta_w - left;
cv::Mat padded_img;
cv::copyMakeBorder(resized_img, padded_img, top, bottom, left, right, cv::BORDER_CONSTANT, padding_color);

return padded_img;
}

cv::Mat DebugEncodingNode::transposeNormalize(const cv::Mat& input)
{
cv::Mat transposed_img;
cv::transpose(input, transposed_img);
cv::Mat normalized_img = transposed_img / 255.0f;

return normalized_img;
}

int main(int argc, char** argv)
{
rclcpp::init(argc, argv);

auto node = std::make_shared();
rclcpp::spin(node);
rclcpp::shutdown();

return 0;
}

.hpp:

#ifndef DEBUG_ENCODING_NODE_HPP_
define DEBUG_ENCODING_NODE_HPP_

include
include
include
include

include <opencv2/opencv.hpp>
include <cv_bridge/cv_bridge.h>
include <rclcpp/rclcpp.hpp>
include <sensor_msgs/msg/image.hpp>
include <isaac_ros_tensor_list_interfaces/msg/tensor_list.hpp>
include <isaac_ros_tensor_list_interfaces/msg/tensor.hpp>
include <isaac_ros_tensor_list_interfaces/msg/tensor_shape.hpp>
include <message_filters/sync_policies/approximate_time.h>
include <message_filters/subscriber.h>
include <message_filters/synchronizer.h>

// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image,
sensor_msgs::msg::Image,
sensor_msgs::msg::Image,
sensor_msgs::msg::Image,
sensor_msgs::msg::Image,
sensor_msgs::msg::Image> SyncPolicy;

class DebugEncodingNode : public rclcpp::Node
{
public:
DebugEncodingNode();

private:
void printTensorShape(
const isaac_ros_tensor_list_interfaces::msg::Tensor& tensor,
const rclcpp::Logger& logger);

void tsCallback(const sensor_msgs::msg::Image::ConstSharedPtr img1,
const sensor_msgs::msg::Image::ConstSharedPtr img2,
const sensor_msgs::msg::Image::ConstSharedPtr img3,
const sensor_msgs::msg::Image::ConstSharedPtr img4,
const sensor_msgs::msg::Image::ConstSharedPtr img5,
const sensor_msgs::msg::Image::ConstSharedPtr img6);

cv::Mat resizeWithPad(const cv::Mat& image, const cv::Size& new_shape, const cv::Scalar& padding_color);

cv::Mat transposeNormalize(const cv::Mat& input);

cv_bridge::CvImagePtr bridge_;
int w_;
int h_;
int c_;
int n_;
int SIZE_FLOAT_;
int network_img_size_;

std::shared_ptr<message_filters::Synchronizer> sync_;

std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> cam_img_sub_1_;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> cam_img_sub_2_;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> cam_img_sub_3_;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> cam_img_sub_4_;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> cam_img_sub_5_;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> cam_img_sub_6_;

rclcpp::Publisher<isaac_ros_tensor_list_interfaces::msg::TensorList>::SharedPtr publisher_;
};

endif // DEBUG_ENCODING_NODE_HPP_

2

Thanks for sharing the code! You’re capturing images from 6 cameras at a time and want to run inference on all of the images in one go. This seems fine to do with one tensor of [n=6, c=3, w=640, h=480] (ncwh). The error indicates that the Tensor msg that you populated the TensorList is missing the tensor with name “input_tensor”. In your code, it looks like it is named “InputTensor” instead.

3

Thanks !
That helped a lot. Besides that, can you give me further advice on the warnings ?

4

The missing tensor would have caused things to go off the rails but are you referring to any specific warning?

1 Like