MoveGroup Freezes on Init, Help!

[azalutsky]

Description

So, I'm trying to run 3 robots simultaneously.

The issue I'm facing is likely due to some issue I've created and I'm trying to see if anyone has any ideas regarding this.

I have two launch files:

Launch file A: Starts RVIZ, Moveit, RobotStatePublisher, and ROS2Control(ers) for my 3 robots.

Launch file B: Starts MoveitC++Interface that I want to launch a movegroup for "whole_system" which is in my srdf file.

If I launch A then B, I get stuck on this line: https://github.com/ros-planning/moveit2/blob/main/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp#L177

My question is can anyone suggest why this is happening all of a sudden and when I had 2 robots it didn't happen. I don't understand why I'm having some kind of deadlock or something and very interested to understand what the heck is happening.

Thanks All!

Your environment

• ROS Distro: Foxy
• OS Version: Ubuntu 18.04
• Source or Binary build? Source
• If binary, which release version?
• If source, which branch? foxy

Steps to reproduce

Tell us how to reproduce this issue. Attempt to provide a working demo, perhaps using Docker.
I have a docker which launches move_group, rviz, ros_controllers for each robot, and a robot state publisher.
In the same docker on a different terminal i launch my moveit c++ node which I want to run my move_group

Something occurs and it doesn't create the movegroup and freezes.

Expected behaviour

It doesn't freeze and continues on to being able to run the robot.

Actual behaviour

freezes at the source code line up above

Backtrace or Console output

[test_plunge_mechanism-1] Link yaskawa_b2/suction had 0 children
[test_plunge_mechanism-1] Link yaskawa_b2/tweezer had 0 children
[test_plunge_mechanism-1] Link iphone_conveyor had 0 children
[test_plunge_mechanism-1] Link iphone_staged had 1 children
[test_plunge_mechanism-1] Link denali had 0 children
[test_plunge_mechanism-1] Link table had 0 children
[test_plunge_mechanism-1] [INFO] [1672886520.337444059] [move_group_interface]: Getting robot model
[test_plunge_mechanism-1] [INFO] [1672886520.337459790] [move_group_interface]: Get JointModelGroup
[test_plunge_mechanism-1] [INFO] [1672886520.337477800] [move_group_interface]: Getting default velocity/acceleration scaling factors
[test_plunge_mechanism-1] [INFO] [1672886520.337491941] [move_group_interface]: Checking if joint model group is chain
[test_plunge_mechanism-1] [INFO] [1672886520.337500187] [move_group_interface]: Creating trajectory publishers
[test_plunge_mechanism-1] [INFO] [1672886520.340018372] [move_group_interface]: Getting shared state monitor
[test_plunge_mechanism-1] [INFO] [1672886520.340081592] [move_group_interface]: Creating actions client: MoveGroup
[test_plunge_mechanism-1] [INFO] [1672886520.343396291] [move_group_interface]: Creating actions client: Wait for action server

^ this is right before line 177
Use gist.github.com to copy-paste the console output or segfault backtrace using gdb.

[azalutsky]

[test_plunge_mechanism-1] [INFO] [1672958324.901535234] [move_group_interface]: Creating actions client: Wait for action server
[test_plunge_mechanism-1] [DEBUG] [1672958324.901621706] [rcl]: Initializing wait set with '0' subscriptions, '2' guard conditions, '0' timers, '0' clients, '0' services
[test_plunge_mechanism-1] [DEBUG] [1672958324.901708856] [rcl]: Waiting without timeout
[test_plunge_mechanism-1] [DEBUG] [1672958324.901724174] [rcl]: Timeout calculated based on next scheduled timer: false
[test_plunge_mechanism-1] [DEBUG] [1672958324.901731306] [rcl]: Guard condition in wait set is ready
[test_plunge_mechanism-1] [DEBUG] [1672958324.901737329] [rcl]: Guard condition in wait set is ready
[test_plunge_mechanism-1] [DEBUG] [1672958324.901748553] [rcl]: Waiting without timeout

Heres a debug output when it freezes. I am curious if these '2' guard conditios in my rcl is an issue. The INFO line "Creating actions client: Wait for action server" is referencing this line: https://github.com/ros-planning/moveit2/blob/main/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp#L177

[JafarAbdi]

Fixed in the latest humble release, see ros2/rmw_fastrtps#650