Inconsistency in making work moveit2 in Humble

[TheConstructAi]

Description

We are getting this issue where sometimes when running moveit2 demo launch in a simulation , sometimes the execution fails and sometimes it doesn't. It changes each time we launch the launch files.

We are getting as working this:

[rviz2-4] [WARN] [1671098946.071992980] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Maybe failed to update robot state, time diff: 1671098872.983s

But its always there, when it fails the execution and when it doesn't, so not sure is siignificative.

We suspect that it might have an effect not setting the use_sim_time. How can this be set with the new way in humble of loading config parameters? In which file we have to put it?

Your environment

• ROS Distro: HUMBLE
• OS Version: e.g. Ubuntu 22.04
• Source or Binary build? Source
• If source, which branch? humble release

Steps to reproduce

Launch a simulation of a robot arm an then the demo.launch.py form moveit.
Source code: https://bitbucket.org/theconstructcore/panda_simulation/src/master/
ROSject PLug and play with all the code and instructions to launch it in notebook: https://app.theconstructsim.com/l/537b5a08/

Launch the following:

cd 
./setup_moveit.sh

cd ~/ros2_ws
source install/setup.bash
ros2 launch panda_robot_bringup start_world.launch.py

cd ~/ros2_ws
source install/setup.bash
ros2 launch panda_robot_bringup panda_robot.launch.py

cd ~/ros2_ws
source install/setup.bash
ros2 launch panda_robot_bringup start_control.launch.py

cd ~/ros2_ws
source install/setup.bash
ros2 launch my_panda_moveit_config move_group.launch.py

cd ~/ros2_ws
source install/setup.bash
ros2 launch my_panda_moveit_config moveit_rviz.launch.py

Expected behaviour

It should be able to plan an execute every time you launch the same launches.

Actual behaviour

It "randomly " fails the execute fase.

Backtrace or Console output

When it doesn't work, no error appears just no moving arrows appear in RVIZ

When it the arrows appear:

I made it appear the arrows by executing:

ros2 daemon stop
ros2 daemon start

Don't know if this makes any sense?
I this resetting some parameters?

Does anyone know what could this be?

Because even when the arrows appear, it fails to execute, giving the error:

[move_group-1] [WARN] [1671100824.609050047] [moveit_ros.trajectory_execution_manager]: Failed to validate trajectory: couldn't receive full current jointstate within 1s
[move_group-1] [INFO] [1671100824.609181350] [moveit_move_group_default_capabilities.move_action_capability]: Solution found but controller failed during execution

[rviz2-1] [INFO] [1671100822.549907729] [move_group_interface]: Plan and Execute request accepted
[rviz2-1] [INFO] [1671100824.609950500] [move_group_interface]: Plan and Execute request aborted
[rviz2-1] [ERROR] [1671100824.651578494] [move_group_interface]: MoveGroupInterface::move() failed or timeout reached
[rviz2-1] [WARN] [1671100825.316650945] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Maybe failed to update robot state, time diff: 1671100373.464s

And when we try to do a dump of the parameters it gives this error:

ros2 param dump /move_group
Traceback (most recent call last):
File "/opt/ros/humble/local/lib/python3.10/dist-packages/rcl_interfaces/msg/_parameter_value.py", line 225, in double_value
assert value >= -1.7976931348623157e+308 and value <= 1.7976931348623157e+308,
AssertionError: The 'double_value' field must be a double in [-1.7976931348623157e+308, 1.7976931348623157e+308]

[JafarAbdi]

Have you tried switching the rmw to cyclonedds? There's a bug in fastdds not working well with callback groups see

[TheConstructAi]

Wow, need to run more tests but the results are promising.
This dds issue seems its affecting everywhere in Humble packages.
We will repost and set the issue solved when we finish my tests, but anyway thanks for the hint @JafarAbdi ;)

[TheConstructAi]

Ok, that was part of the issue, the second part was related to the use_sim_time.
If you don't set it in this file: MOD for use sim time True
It wont execute any trajectory.

We have posted this question to ask how this can be set without hardcoding it using the new config load param system, but I'll consider this as answered not to extend more.

If someone comes across this, just let you know that it was solved by two things:

1. adding before each launch file command in the shell:

export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

2. Modified this file so that we set use_sim_time to True ( only applicable obviously to simulation ): launches.py

Thanks again for the help @JafarAbdi

[henningkayser]

Closing this, updates to the DDS situation will be posted to #1874