Control Node Failure when Camera topic is added in Rviz

[Abhishek8857]

As soon as you add a camera visualisation topic in Rviz, the control node fails and gives the follwing log. However, there is no failure for the depth camera visualisation.

[rviz2-10] [INFO] [1741255195.918042277] [rviz2]: Stereo is NOT SUPPORTED
[ros2_control_node-1] terminate called after throwing an instance of 'std::runtime_error'
[ros2_control_node-1] what(): timeout detected: BaseCyclicClient::RefreshFeedback
[ros2_control_node-1]
[ros2_control_node-1] Stack trace (most recent call last) in thread 20534:
[ros2_control_node-1] #17 Object "", at 0xffffffffffffffff, in
[ros2_control_node-1] #16 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x753417e9084f, in
[ros2_control_node-1] #15 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x753417dfeac2, in
[ros2_control_node-1] #14 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x75341808f252, in
[ros2_control_node-1] #13 Object "/colcon_ws/build/controller_manager/ros2_control_node", at 0x653211a3eedf, in main::{lambda()#1}::operator()() const
[ros2_control_node-1] #12 Object "/colcon_ws/build/controller_manager/libcontroller_manager.so", at 0x7534184ee074, in controller_manager::ControllerManager::write(rclcpp::Time const&, rclcpp::Duration const&)
[ros2_control_node-1] #11 Object "/colcon_ws/build/hardware_interface/libhardware_interface.so", at 0x753417c4badc, in hardware_interface::ResourceManager::write(rclcpp::Time const&, rclcpp::Duration const&)
[ros2_control_node-1] #10 Object "/colcon_ws/build/hardware_interface/libhardware_interface.so", at 0x753417c8330f, in hardware_interface::System::write(rclcpp::Time const&, rclcpp::Duration const&)
[ros2_control_node-1] #9 Object "/colcon_ws/build/kortex_driver/libkortex_driver.so", at 0x75341084d909, in kortex_driver::KortexMultiInterfaceHardware::write(rclcpp::Time const&, rclcpp::Duration const&)
[ros2_control_node-1] #8 Object "/colcon_ws/build/kortex_driver/libkortex_driver.so", at 0x75341083f348, in kortex_driver::KortexMultiInterfaceHardware::sendJointCommands() [clone .cold]
[ros2_control_node-1] #7 Object "/colcon_ws/build/kortex_driver/libkortex_driver.so", at 0x7534109bf6c3, in Kinova::Api::BaseCyclic::BaseCyclicClient::RefreshFeedback(unsigned int, Kinova::Api::RouterClientSendOptions const&)
[ros2_control_node-1] #6 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7534180614d7, in __cxa_throw
[ros2_control_node-1] #5 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x753418061276, in std::terminate()
[ros2_control_node-1] #4 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x75341806120b, in
[ros2_control_node-1] #3 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x753418055b9d, in
[ros2_control_node-1] #2 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x753417d927f2, in abort
[ros2_control_node-1] #1 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x753417dac475, in raise
[ros2_control_node-1] #0 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x753417e009fc, in pthread_kill
[ros2_control_node-1] Aborted (Signal sent by tkill() 20427 0)
[ERROR] [ros2_control_node-1]: process has died [pid 20427, exit code -6, cmd '/colcon_ws/install/controller_manager/lib/controller_manager/ros2_control_node --ros-args --params-file /tmp/launch_params_bm0rckuh --params-file /colcon_ws/install/kinova_gen3_7dof_robotiq_2f_85_moveit_config/share/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ros2_controllers.yaml -r /controller_manager/robot_description:=/robot_description'].

[aalmrad]

Hello,

I can see that you are attempting to launch moveit and start the camera streams at the same time, therefore you might be overloading your CPU and/or your network. If you start the camera streams and launch moveit separatly, do you still face issues?

Best,
Abed

[Abhishek8857]

The problem i face is if i launch vision with "ros2 launch kinova_vision kinova_vision.launch.py" it runs without any issue

but if I launch it with :
"ros2 launch kinova_vision kinova_vision.launch.py
depth_registration:=true
launch_depth:=true
max_color_pub_rate:=5.0
max_depth_pub_rate:=5.0 "

there is control node failure

[aalmrad]

Thank you for the update, do you face any issue when running the command with the default argument values?

[Abhishek8857]

Sorry for the delay, i didn't have access to the robot. The code works perfectly when i just run "ros2 launch kinova_vision kinova_vision.launch.py" but when i activate teh depth registrations there is the control node failure

[aalmrad]

@Abhishek8857 ,

I tried to replicate the issue on my side on a Gen3 7 DoF by starting the robot.launch.py file from the moveit package
and kinova_vision.launch.py from the kinova_vision package with the depth_registration set to true and I was able to command the robot while visualizing the camera streams and point cloud. Please let me know if you are following different steps and I will reattempt to replicate the issue on my side, otherwise you need to make sure that you have enough computational power to avoid the ros2_control node crashes.

[Abhishek8857]

@aalmrad I am actually using a docker image with ros2 kortex, kortex vision, moveit and moveit task constructor as submodules

This is my docker file:

ARG ROS_DISTRO=humble
FROM osrf/ros:${ROS_DISTRO}-desktop

# Source the workspace
RUN echo "source /colcon_ws/install/setup.bash" >> ~/.bashrc

# Replace /bin/sh with /bin/bash
RUN rm /bin/sh && ln -s /bin/bash /bin/sh

# Set the ROS Domain ID
ENV ROS_DOMAIN_ID=0
ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

# Install required dependencies
RUN apt-get update && apt-get install -y \
    wget \
    lsb-release \
    gnupg \
    build-essential \
    cmake \
    ament-cmake \
    python3-pip \
    python3-colcon-common-extensions \
    python3-vcstool \
    ros-humble-joint-state-publisher-gui \
    ros-humble-rmw-cyclonedds-cpp \
    ros-humble-kortex-bringup \
    ros-humble-ros2-control \
    ros-humble-ros2-controllers \
    ros-humble-xacro \
    ros-humble-topic-based-ros2-control \
    ros-humble-tf-transformations \
    ros-humble-ros-gz-bridge \
    ros-humble-gazebo-ros-pkgs \
    ros-humble-gazebo-ros2-control \
    ros-humble-rviz2 \
    ros-humble-depth-image-proc \
    gstreamer1.0-tools \
    gstreamer1.0-libav \    
    gstreamer1.0-plugins-good \
    gstreamer1.0-plugins-base \
    libgstreamer1.0-dev \
    libgstreamer-plugins-base1.0-dev \
    libgstreamer-plugins-good1.0-dev \
    software-properties-common \
    libignition-transport11-dev \
    libgflags-dev \
    ros-humble-kinematics-interface-kdl \
    ros-humble-test-msgs \
    libcap-dev \
    libsm6 \
    libxext6 \
    libgl1-mesa-glx \
    libglib2.0-0 \
    mesa-utils \
    libqt5gui5 \
    ros-humble-kinova-gen3-7dof-robotiq-2f-85-moveit-config \
    ros-humble-kinova-gen3-6dof-robotiq-2f-85-moveit-config \
    && apt-get clean && rm -rf /var/lib/apt/lists/*
          
    
# Install igntion gazebo
RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -sc) main" > /etc/apt/sources.list.d/gazebo-stable.list'
RUN wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add -
RUN apt-get update && apt-get install -y libignition-gazebo6 libignition-gazebo6-dev

# Fix missing update
RUN apt-get update --fix-missing -y

# Copy the entire colcon_ws  and overlay_ws directory with the submodule into the Docker image
COPY colcon_ws/ /colcon_ws/
WORKDIR /colcon_ws/src/

# Update package lists and import MoveIt repositories based on the specified ROS distribution
RUN apt-get update && \
    for repo in moveit2/moveit2.repos $(f="moveit2/moveit2_$ROS_DISTRO.repos"; test -r $f && echo $f); do \
        vcs import < "$repo"; \
    done && \
    rosdep install -r --from-paths . --ignore-src --rosdistro $ROS_DISTRO -y

WORKDIR /colcon_ws/

# Add RMW implementation to the bashrc file for persistent sourcing
RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> ~/.bashrc

# Import additional repositories using vcs

RUN vcs import src --skip-existing --input src/required.repos

RUN rosdep update
RUN sudo apt-get update
RUN rosdep install --from-paths . src --ignore-src -r -y

# Build the workspace with resource management
RUN source /opt/ros/humble/setup.bash && \
    MAKEFLAGS="-j4 -l2" colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 3 --symlink-install --executor sequential


RUN apt-get update && apt-get upgrade -y

# Copy cyclonedds config files
# COPY cyclonedds/config.xml /config.xml
# COPY cyclonedds/10-cyclone-max.conf /etc/sysctl.d/10-cyclone-max.conf

# Copy entrypoint scripts and make them executable
COPY entrypoint_scripts/ /entrypoint_scripts/
RUN chmod +x /entrypoint_scripts/*.sh

# Copy contents in overlay ws
COPY overlay_ws/ /overlay_ws/
WORKDIR /overlay_ws/

RUN rosdep install --from-paths src --ignore-src -r -y

RUN source /colcon_ws/install/setup.bash && \
    colcon build --event-handlers desktop_notification- status- --cmake-args -DCMAKE_BUILD_TYPE=Release

# Set the entrypoint script (modify the entrypoint script as needed)
# ENTRYPOINT ["/entrypoint_scripts/entrypoint_base_node.sh"]

[aalmrad]

@Abhishek8857,

Do you face issues if you run everything following each repository's instructions without a docker file?

[github-actions]

As part of Kinova's beta testing: Claude is analyzing this issue and will attempt an automated fix. A pull request will be opened if changes are made.

[github-actions]

Claude has submitted a fix attempt: #401

Please review the proposed modifications and test them on your end, then provide feedback.
A member of Kinova's team will follow up on this issue and take the appropriate next steps.