Skip to content

srv_kinematics_plugin generates deadlock when calling the external IK service #3633

@marco-faroni

Description

@marco-faroni

Description

I'm using the srv_kinematics_plugin to call an external service server with a custom IK solver. When I call the compute_ik service, the move_group node correctly enters the callback, but it never exits. Apparently there is a deadlock caused by the compute_ik callback and the client (calling the external IK) being in the same callback_group. I've tried to assign the client callback to a Reentrant callback group and it works properly (see a fork here: https://github.com/marco-faroni/moveit2/tree/humble).

ROS Distro

Humble

OS and version

Ubuntu 22.04

Source or binary build?

Binary

If binary, which release version?

2.5.9-1jammy.20251119.014812

If source, which branch?

No response

Which RMW are you using?

None

Steps to Reproduce

  1. Create a MoveIt kinematics plugin based on SrvKinematicsPlugin. You can find an example here: https://github.com/marco-faroni/ik_solver_moveit_adapter
  2. Launch move_group.
  3. Request IK via MoveIt. E.g.,
ros2 service call /compute_ik moveit_msgs/srv/GetPositionIK "ik_request:
  group_name: 'manipulator'
  robot_state:
    joint_state:
      header:
        stamp:
          sec: 0
          nanosec: 0
        frame_id: ''
      name: []
      position: []
      velocity: []
      effort: []
    multi_dof_joint_state:
      header:
        stamp:
          sec: 0
          nanosec: 0
        frame_id: ''
      joint_names: []
      transforms: []
      twist: []
      wrench: []
    attached_collision_objects: []
    is_diff: false
  constraints:
    name: ''
    joint_constraints: []
    position_constraints: []
    orientation_constraints: []
    visibility_constraints: []
  avoid_collisions: false
  ik_link_name: ''
  pose_stamped:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: ''
    pose:
      position:
        x: 1.0
        y: 1.0
        z: 0.5
      orientation:
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
  ik_link_names: []
  pose_stamped_vector: []
  timeout:
    sec: 0
    nanosec: 0"

Expected behavior

The service should return a response containing the IK solution (or NO_IK_SOLUTION error code).

Actual behavior

The external IK server correctly enters the callback, but it never returns a response. Apparently there is a deadlock caused by the compute_ik callback and the client (calling the external IK) being in the same callback_group. I've tried to assign the client callback to a Reentrant callback group and it works properly (see a fork here: https://github.com/marco-faroni/moveit2/tree/humble)

Backtrace or Console output

No response

Metadata

Metadata

Assignees

No one assigned

    Labels

    bugSomething isn't working

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions