[rqt jtc] fails to receive state since new jtc ctrl msgs

[gwalck]

Describe the bug

following #557 jtc is not working anymore to read feedback, and failed at sending commands

KeyError: 'position'
Traceback (most recent call last):
  File "~/workspaces/ws/build/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py", line 423, in _update_cmd_cb
    pos = self._joint_pos[name]["position"]

Expected behavior
RQT JTC should get its information from the new "controller_state" topic in the new JTC message.

Environment (please complete the following information):

• OS: ubuntu 22.04
• Version master of ros2_controllers against rolling

A PR is ready. standby

[bmagyar]

Ah thank you! I started looking into reproducing this a few days ago

…

On Fri, 2 Jun 2023, 15:14 gwalck, ***@***.***> wrote: *Describe the bug* following #557 <#557> jtc is not working anymore to read feedback, and failed at sending commands KeyError: 'position' Traceback (most recent call last): File "~/workspaces/ws/build/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py", line 423, in _update_cmd_cb pos = self._joint_pos[name]["position"] *Expected behavior* RQT JTC should get its information from the new "controller_state" topic in the new JTC message. *Environment (please complete the following information):* - OS: ubuntu 22.04 - Version master of ros2_controllers against rolling A PR is ready. standby — Reply to this email directly, view it on GitHub <#642>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AA24PYNPKNBKFOVUIL2LIBDXJHYNPANCNFSM6AAAAAAYYMDB2A> . You are receiving this because you are subscribed to this thread.Message ID: ***@***.***>

[bmagyar]

Do we also have this issue in humble?

[gwalck]

@bmagyar if jtc new control messages were ported back to humble, maybe, I did not test.

[gwalck]

as far as I can see when looking at code in humble and testing a modified rrbot launch file to start a jtc instead of the forward controller on humble branch of ros2_control_demos

ros2 launch ros2_control_demo_example_1 rrbot.launch.py

the jtc controller of humble is publishing to both topics (state and controller_state) with a message containing both old and new structures, so the RQT JTC in humble is fine with it (well the rrbot has no joint limits I had to add some to get he RQT JTC to accept display the sliders).

[destogl]

We could also update humble but this could also confuse people...

[destogl]

Maybe to add a note that RQT plugin is using only new topic because we want people using humble to transition to the new one

[gwalck]

What I understood is that humble was supposed to handle both, so we should not break RQT_JTC there. But now RQT_JTC is broken in rolling because in rolling there is only one topic accepted and one type of msg accepted.

[mpollayil]

Hi all,

At our company we are also encountering a problem that seems to be related to this issue. This can be reproduced by running the moveit_resources Fanuc example from this repo: ros2 launch moveit_resources_fanuc_moveit_config demo.launch.py

We get the following segmentation fault that is related to the new control_msgs::msg::JointTrajectoryControllerState:

[ros2_control_node-5] Stack trace (most recent call last) in thread 1941:
[ros2_control_node-5] #14   Object "", at 0xffffffffffffffff, in 
[ros2_control_node-5] #13   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f74bca149ff, in 
[ros2_control_node-5] #12   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f74bc982b42, in 
[ros2_control_node-5] #11   Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f74bcc122b2, in 
[ros2_control_node-5] #10   Object "/opt/ros/humble/lib/libjoint_trajectory_controller.so", at 0x7f749ce17d92, in realtime_tools::RealtimePublisher<control_msgs::msg::JointTrajectoryControllerState_<std::allocator<void> > >::publishingLoop()
[ros2_control_node-5] #9    Object "/opt/ros/humble/lib/librcl.so", at 0x7f74bc78cadd, in 
[ros2_control_node-5] #8    Object "/opt/ros/humble/lib/librmw_fastrtps_shared_cpp.so", at 0x7f74bad77853, in rmw_fastrtps_shared_cpp::__rmw_publish(char const*, rmw_publisher_s const*, void const*, rmw_publisher_allocation_s*)
[ros2_control_node-5] #7    Object "/opt/ros/humble/lib/libfastrtps.so.2.6.4", at 0x7f74ba83f768, in eprosima::fastdds::dds::DataWriterImpl::write(void*)
[ros2_control_node-5] #6    Object "/opt/ros/humble/lib/libfastrtps.so.2.6.4", at 0x7f74ba83f72f, in eprosima::fastdds::dds::DataWriterImpl::create_new_change(eprosima::fastrtps::rtps::ChangeKind_t, void*)
[ros2_control_node-5] #5    Object "/opt/ros/humble/lib/libfastrtps.so.2.6.4", at 0x7f74ba83f652, in eprosima::fastdds::dds::DataWriterImpl::create_new_change_with_params(eprosima::fastrtps::rtps::ChangeKind_t, void*, eprosima::fastrtps::rtps::WriteParams&)
[ros2_control_node-5] #4    Object "/opt/ros/humble/lib/libfastrtps.so.2.6.4", at 0x7f74ba83e61b, in eprosima::fastdds::dds::DataWriterImpl::perform_create_new_change(eprosima::fastrtps::rtps::ChangeKind_t, void*, eprosima::fastrtps::rtps::WriteParams&, eprosima::fastrtps::rtps::InstanceHandle_t const&)
[ros2_control_node-5] #3    Object "/opt/ros/humble/lib/librmw_fastrtps_shared_cpp.so", at 0x7f74bad87050, in 
[ros2_control_node-5] #2    Object "/opt/ros/humble/lib/librmw_fastrtps_cpp.so", at 0x7f74badd8cf3, in 
[ros2_control_node-5] #1    Object "/opt/ros/humble/lib/libcontrol_msgs__rosidl_typesupport_fastrtps_cpp.so", at 0x7f749cc7270e, in 
[ros2_control_node-5] #0    Object "/opt/ros/humble/lib/libcontrol_msgs__rosidl_typesupport_fastrtps_cpp.so", at 0x7f749cc726b2, in control_msgs::msg::typesupport_fastrtps_cpp::get_serialized_size(control_msgs::msg::JointTrajectoryControllerState_<std::allocator<void> > const&, unsigned long)
[ros2_control_node-5] Segmentation fault (Invalid permissions for mapped object [0x7f7468021008])
[rviz2-1] [INFO] [1686168991.826719594] [rviz2]: Stereo is NOT SUPPORTED
[ERROR] [ros2_control_node-5]: process has died [pid 1816, exit code -11, cmd '/opt/ros/humble/lib/controller_manager/ros2_control_node --ros-args --params-file /tmp/launch_params_h6swq6_8 --params-file /home/ros/install/moveit_resources_fanuc_moveit_config/share/moveit_resources_fanuc_moveit_config/config/ros2_controllers.yaml'].

This makes ros2_control crash and it always happens immediately after the fanuc_controller, that is of type joint_trajectory_controller/JointTrajectoryController, is configured and activated: Configured and activated fanuc_controller.

Any idea about how to solve this? @bmagyar @gwalck @destogl

We are working within a Docker container that pulls this image.

[destogl]

@mpollayil this looks like a different issue. Can you give us more informations about your setup?

What are you using from binaries, what are you using from source. This might points us to know what is happening.

[mpollayil]

Hi Denis,

thanks for your prompt reply.

@mpollayil this looks like a different issue. Can you give us more informations about your setup?

Would you like me to open a separate thread for this?

What are you using from binaries, what are you using from source. This might points us to know what is happening.

We are not installing anything from source except for the moveit_resources package, installed with

git clone https://github.com/ros-planning/moveit_resources.git
colcon build --packages-select moveit_resources_fanuc_description moveit_resources_fanuc_moveit_config

Our Docker file pulls this image and then only installs the following:

# Install necessary ros2 packages
RUN sudo apt update -y \
    && sudo apt install -y ros-humble-controller-manager \
    && sudo apt install -y ros-humble-position-controllers \
    && sudo apt install -y ros-humble-gripper-controllers \
    && sudo apt install -y ros-humble-joint-trajectory-controller\
    && sudo apt install -y ros-humble-joint-state-broadcaster\
    && sudo apt install -y ros-humble-rqt-*   

Hope these information are enough. Please let me know if I should open another thread for this issue.

[bmagyar]

To me those error messages seem like a convoluted way of saying "ABI incompatibility of messages". That being said I'm not really intimate with fastrtps to understand it's errors well.

[destogl]

What happens if you don't start MoveIt part?

It could be that the messages are not compatible due recent changes or that libraries are not on the correct versions. Have you updated or created Docker in the last few days?

[mpollayil]

@destogl A very minimal launch file like the following one would still make ros2_control node crash:

import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():

    moveit_config = (
        MoveItConfigsBuilder("moveit_resources_fanuc")
        .robot_description(file_path="config/fanuc.urdf.xacro")
        .robot_description_semantic(file_path="config/fanuc.srdf")
        .trajectory_execution(file_path="config/moveit_controllers.yaml")
        .to_moveit_configs()
    )

    # ros2_control using FakeSystem as hardware
    ros2_controllers_path = os.path.join(
        get_package_share_directory("moveit_resources_fanuc_moveit_config"),
        "config",
        "ros2_controllers.yaml",
    )

    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[moveit_config.robot_description, ros2_controllers_path],
        output="both",
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_state_broadcaster",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    fanuc_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "fanuc_controller",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    return LaunchDescription(
        [
            ros2_control_node,
            joint_state_broadcaster_spawner,
            fanuc_controller_spawner,
        ]
    )

It just runs ros2_control and spawns the two controllers. So, we believe this issue might not be related to MoveIt... Can it be related to simulating hardware with the ros2_control FakeSystem?

Regarding Docker, we did not create or update it, but we rebuilt the image from scratch a few days ago and since that day we started having these issues. With the old image everything was working perfectly.

[destogl]

Does this also happens with our examples?

This could be ABI incompatibility, so you can try to compile controller in your setup.

If not that is the issue in your setup. To help you there, we have to replicate the setup - this is then "professional support" category. If you re interest, write request on [email protected]

[christophfroehlich]

The original problem was solved for rolling with #643.
-> Discussions about backporting to humble into #659
-> @mpollayil if your problem still persists, feel free to open a new issue.