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

Failed to load node 'isaac_ros_image_rectifier_right' of type 'nvidia::isaac_ros::image_proc::RectifyNode' in container: Component constructor threw an exception: intraprocess communication allowed only with volatile durability #45

Open
gc625-kodifly opened this issue Mar 6, 2024 · 0 comments

Comments

@gc625-kodifly
Copy link

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant