-
Notifications
You must be signed in to change notification settings - Fork 31
Description
Hi,
currently developing a project using the official docker container provided on an x86 host running ubuntu 20.04, and because of the project details I have to use an rtsp camera. I have the following node that uses gstreamer to read in the camera stream and publishes it.
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"
#include "camera_info_manager/camera_info_manager.hpp"
namespace camera_interface_cpp
{
class ImagePublisher : public rclcpp::Node
{
public:
ImagePublisher(const rclcpp::NodeOptions &options) : Node("ImagePublisher", options)
{
this->declare_parameter<std::string>("rtsp_username", "admin");
this->declare_parameter<std::string>("rtsp_password", "password");
this->declare_parameter<std::string>("rtsp_ip", "ip");
this->declare_parameter<int>("rtsp_fps", 20);
this->declare_parameter<std::string>("camera_name", "camera");
this->declare_parameter<std::string>("calibration_url", "");
std::string rtsp_username, rtsp_password, rtsp_ip, camera_name, calibration_url;
int rtsp_fps;
this->get_parameter("rtsp_username", rtsp_username);
this->get_parameter("rtsp_password", rtsp_password);
this->get_parameter("rtsp_ip", rtsp_ip);
this->get_parameter("rtsp_fps", rtsp_fps);
this->get_parameter("camera_name", camera_name);
this->get_parameter("calibration_url", calibration_url);
info_manager_ = std::make_shared<camera_info_manager::CameraInfoManager>(this, camera_name);
info_manager_->loadCameraInfo(calibration_url);
auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).durability_volatile();
camera_info_pub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("camera_info", qos);
publisher_ = this->create_publisher<sensor_msgs::msg::Image>("image_topic", qos);
timer_ = this->create_wall_timer(
std::chrono::milliseconds(1000 / rtsp_fps),
std::bind(&ImagePublisher::publish_frame, this));
// OpenCV VideoCapture with GStreamer pipeline
std::string rtsp_url = "rtsp://" + rtsp_username + ":" + rtsp_password + "@" + rtsp_ip + ":554";
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "RTSP URL: %s", rtsp_url.c_str());
pipeline = "rtspsrc location=" + rtsp_url +
" latency=0 ! "
"queue ! "
"rtph264depay ! "
"h264parse ! "
"avdec_h264 ! "
"videoconvert ! "
"appsink";
cap_.open(pipeline, cv::CAP_GSTREAMER);
if (!cap_.isOpened())
{
RCLCPP_ERROR(this->get_logger(), "Failed to open the camera feed");
}
}
private:
void publish_frame()
{
if (!cap_.isOpened())
{
RCLCPP_ERROR(this->get_logger(), "Failed to open the camera feed");
cap_.open(pipeline, cv::CAP_GSTREAMER);
return;
}
cv::Mat frame;
if (!cap_.read(frame))
{
RCLCPP_ERROR(this->get_logger(), "Failed to read frame from the camera feed");
cap_.open(pipeline, cv::CAP_GSTREAMER);
return;
}
rclcpp::Time now = this->now();
sensor_msgs::msg::CameraInfo camera_info_msg = info_manager_->getCameraInfo();
camera_info_msg.header.frame_id = "camera_frame"; // Set to appropriate frame id
camera_info_msg.header.stamp = now;
camera_info_pub_->publish(camera_info_msg);
auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toImageMsg();
msg->header.stamp = now;
msg->header.frame_id = "camera_frame"; // Ensure this matches camera_info_msg
publisher_->publish(*msg);
}
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
cv::VideoCapture cap_;
std::string pipeline;
std::shared_ptr<camera_info_manager::CameraInfoManager> info_manager_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_pub_;
};
}
#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(camera_interface_cpp::ImagePublisher)
i installed the package with sudo apt-get install -y ros-humble-isaac-ros-image-proc and would like to use the rectify node from the package. Since I want to take advantage of composables Nodes, I have my launch file like so:
import launch
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
params = os.path.join(
get_package_share_directory('bringup'),
'config',
'params.yaml'
)
right_camera_container = ComposableNodeContainer(
name='right_camera_container',
namespace='',
package='rclcpp_components',
executable='component_container',
composable_node_descriptions=[
ComposableNode(
package='camera_interface_cpp',
plugin='camera_interface_cpp::ImagePublisher',
name='image_publisher_right',
remappings=[
('image_topic', 'image/right/raw'),
('camera_info', 'image/right/camera_info')
],
parameters=[params],
extra_arguments=[{'use_intra_process_comms': True}]),
ComposableNode(
package='isaac_ros_image_proc',
plugin='nvidia::isaac_ros::image_proc::RectifyNode',
name='isaac_ros_image_rectifier_right',
parameters=[{
'output_height': 720,
'output_width': 1280,
}],
remappings=[
('/image_raw', '/image/right/raw'),
('/camera_info', '/image/right/camera_info'),
],
extra_arguments=[{'use_intra_process_comms': True}]
)
],
)
return launch.LaunchDescription([right_camera_container])
However, the rectifyNode is unable to launch, and returns the following error:
[component_container-1] [INFO] [1709698554.895557930] [right_camera_container]: Load Library: /opt/ros/humble/lib/librectify_node.so
[component_container-1] [INFO] [1709698554.924203888] [right_camera_container]: Found class: rclcpp_components::NodeFactoryTemplate<nvidia::isaac_ros::image_proc::RectifyNode>
[component_container-1] [INFO] [1709698554.924243283] [right_camera_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nvidia::isaac_ros::image_proc::RectifyNode>
[component_container-1] [INFO] [1709698554.926704587] [NitrosContext]: [NitrosContext] Creating a new shared context
[component_container-1] [INFO] [1709698554.926755312] [isaac_ros_image_rectifier_right]: [NitrosNode] Initializing NitrosNode
[component_container-1] [INFO] [1709698554.926886815] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/std/libgxf_std.so
[component_container-1] [INFO] [1709698554.934141138] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_gxf_helpers.so
[component_container-1] [INFO] [1709698554.938776601] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_sight.so
[component_container-1] [INFO] [1709698554.945355173] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_atlas.so
[component_container-1] [INFO] [1709698554.954075398] [NitrosContext]: [NitrosContext] Loading application: '/opt/ros/humble/share/isaac_ros_nitros/config/type_adapter_nitros_context_graph.yaml'
[component_container-1] [INFO] [1709698554.955214632] [NitrosContext]: [NitrosContext] Initializing application...
[component_container-1] [INFO] [1709698554.958184775] [NitrosContext]: [NitrosContext] Running application...
[component_container-1] [INFO] [1709698554.958487827] [isaac_ros_image_rectifier_right]: [NitrosNode] Starting NitrosNode
[component_container-1] [INFO] [1709698554.958500674] [isaac_ros_image_rectifier_right]: [NitrosNode] Loading built-in preset extension specs
[component_container-1] [INFO] [1709698554.960597435] [isaac_ros_image_rectifier_right]: [NitrosNode] Loading built-in extension specs
[component_container-1] [INFO] [1709698554.960622025] [isaac_ros_image_rectifier_right]: [NitrosNode] Loading preset extension specs
[component_container-1] [INFO] [1709698554.962840165] [isaac_ros_image_rectifier_right]: [NitrosNode] Loading extension specs
[component_container-1] [INFO] [1709698554.962854198] [isaac_ros_image_rectifier_right]: [NitrosNode] Loading generator rules
[component_container-1] [INFO] [1709698554.963056459] [isaac_ros_image_rectifier_right]: [NitrosNode] Loading extensions
[component_container-1] [INFO] [1709698554.963094582] [isaac_ros_image_rectifier_right]: [NitrosContext] Loading extension: gxf/lib/libgxf_message_compositor.so
[component_container-1] [INFO] [1709698554.963713516] [isaac_ros_image_rectifier_right]: [NitrosContext] Loading extension: gxf/lib/cuda/libgxf_cuda.so
[component_container-1] [INFO] [1709698554.964284843] [isaac_ros_image_rectifier_right]: [NitrosContext] Loading extension: gxf/lib/image_proc/libgxf_tensorops.so
[component_container-1] [INFO] [1709698554.980244107] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/multimedia/libgxf_multimedia.so
[component_container-1] [INFO] [1709698554.981053892] [isaac_ros_image_rectifier_right]: [NitrosNode] Loading graph to the optimizer
[component_container-1] [INFO] [1709698554.982937104] [isaac_ros_image_rectifier_right]: [NitrosNode] Running optimization
[component_container-1] [INFO] [1709698554.998453587] [isaac_ros_image_rectifier_right]: [NitrosNode] Obtaining graph IO group info from the optimizer
[component_container-1] [INFO] [1709698554.999132926] [isaac_ros_image_rectifier_right]: [NitrosNode] Creating negotiated publishers/subscribers
[component_container-1] [INFO] [1709698555.002886834] [isaac_ros_image_rectifier_right]: [NitrosNode] Terminating the running application
[component_container-1] [INFO] [1709698555.002912379] [isaac_ros_image_rectifier_right]: [NitrosContext] Interrupting GXF...
[component_container-1] 2024-03-06 12:15:55.002 ERROR gxf/std/program.cpp@533: Attempted interrupting when not running (state=0).
[component_container-1] 2024-03-06 12:15:55.002 ERROR gxf/core/runtime.cpp@1400: Graph interrupt failed with error: GXF_INVALID_EXECUTION_SEQUENCE
[component_container-1] [ERROR] [1709698555.002926748] [isaac_ros_image_rectifier_right]: [NitrosContext] GxfGraphInterrupt Error: GXF_INVALID_EXECUTION_SEQUENCE
[component_container-1] [INFO] [1709698555.002945449] [isaac_ros_image_rectifier_right]: [NitrosContext] Waiting on GXF...
[component_container-1] [INFO] [1709698555.002949138] [isaac_ros_image_rectifier_right]: [NitrosContext] Deinitializing...
[component_container-1] [INFO] [1709698555.002952421] [isaac_ros_image_rectifier_right]: [NitrosContext] Destroying context
[component_container-1] [INFO] [1709698555.003116350] [isaac_ros_image_rectifier_right]: [NitrosNode] Application termination done
[component_container-1] [ERROR] [1709698555.004670327] [right_camera_container]: Component constructor threw an exception: intraprocess communication allowed only with volatile durability
[ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'isaac_ros_image_rectifier_right' of type 'nvidia::isaac_ros::image_proc::RectifyNode' in container '/right_camera_container': Component constructor threw an exception: intraprocess communication allowed only with volatile durability
Note that if i disable use_intra_process_comms, everything launches fine, however I would assume this needs to be on in order to minimize data transfer overhead between nodes?
I have already set the qos of the publishers:
auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).durability_volatile();
to volatile, so im unsure what is the solution here. Thanks