Troubleshooting

Problem: I get compilation errors in MoveIt2 packages when building my workspace. Example shown below:

--- stderr: moveit_planners_ompl
/usr/bin/ld: CMakeFiles/moveit_generate_state_database.dir/scripts/generate_state_database.cpp.o: in function `main':
generate_state_database.cpp:(.text.startup+0x76f): undefined reference to `planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<tf2_ros::Buffer> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)'
collect2: error: ld returned 1 exit status
make[2]: *** [ompl_interface/CMakeFiles/moveit_generate_state_database.dir/build.make:313: ompl_interface/generate_state_database] Error 1
make[1]: *** [CMakeFiles/Makefile2:328: ompl_interface/CMakeFiles/moveit_generate_state_database.dir/all] Error 2
make: *** [Makefile:141: all] Error 2

Solution: Uninstall conflicting MoveIt2 binary packages and reinstall MoveIt Studio workspace dependencies:

sudo apt remove --purge ros-rolling-moveit*
cd $STUDIO_WS/src
rosdep install --ignore-src --from-paths . -y -r --skip-keys=librealsense2

Problem: MoveIt Studio and other ROS processes are throwing network errors along the lines of ddsi_udp_conn_write_failed

Solution: Try restarting the ROS daemon with ros2 daemon stop, then ros2 daemon start. If that doesn’t work, try rebooting.


Problem: I can ping the other computer but they can’t communicate via ros2 run demo_nodes_py talker + ros2 run demo_nodes_py listener

Solution: Check that ~/.ros/cyclonedds.xml files on each machine are pointing to the right network interface adapter, say if you changed USB ethernet adapters.


Problem: I cannot connect to the Realsense

Solution: Reconnect the Realsense and then check if you can view it in the realsense program: if the program is not on your system, follow the Realsense Drivers instructions. The expected set of realsense packages is:

$ apt list --installed | grep realsense

librealsense2-dbg/focal,now 2.48.0-0~realsense0.4976 amd64 [installed]
librealsense2-dev/focal,now 2.48.0-0~realsense0.4976 amd64 [installed]
librealsense2-dkms/focal,now 1.3.18-0ubuntu1 all [installed]
librealsense2-gl-dbg/focal 2.48.0-0~realsense0.4976 amd64
librealsense2-gl-dev/focal 2.48.0-0~realsense0.4976 amd64
librealsense2-gl/focal,now 2.48.0-0~realsense0.4976 amd64 [installed,automatic]
librealsense2-net-dbg/focal 2.48.0-0~realsense0.4976 amd64
librealsense2-net-dev/focal 2.48.0-0~realsense0.4976 amd64
librealsense2-net/focal,now 2.48.0-0~realsense0.4976 amd64 [installed,automatic]
librealsense2-udev-rules/focal,now 2.48.0-0~realsense0.4976 amd64 [installed,automatic]
librealsense2-utils/focal,now 2.48.0-0~realsense0.4976 amd64 [installed]
librealsense2/focal,now 2.48.0-0~realsense0.4976 amd64 [installed,automatic]
ros-humble-librealsense2-dbgsym/focal 2.48.0-1focal.20210701.133935 amd64
ros-humble-librealsense2/focal,now 2.48.0-1focal.20210701.133935 amd64 [installed]
ros-humble-realsense-msgs-dbgsym/focal 2.0.8-2focal.20210630.230828 amd64
ros-humble-realsense-msgs/focal 2.0.8-2focal.20210630.230828 amd64
ros-humble-realsense2-camera-dbgsym/focal 3.2.2-1focal.20210705.140737 amd64
ros-humble-realsense2-camera-msgs-dbgsym/focal 3.2.2-1focal.20210705.135802 amd64
ros-humble-realsense2-camera-msgs/focal 3.2.2-1focal.20210705.135802 amd64
ros-humble-realsense2-camera/focal 3.2.2-1focal.20210705.140737 amd64
ros-humble-realsense2-description/focal 3.2.2-1focal.20210705.140354 amd64

Problem: CMAKE_MODULE_PATH issues, humble being sourced before/after another distro, etc

Solution: Start digging through your environment variables before and after sourcing your workspace to see what is going on. In particular:

AMENT_PREFIX_PATH
CMAKE_PREFIX_PATH
COLCON_PREFIX_PATH
CMAKE_MODULE_PATH
LD_LIBRARY_PATH
PATH

If a path is unexpectedly polluted in a fresh terminal and you have checked that your .bashrc settings are not the cause and you recently changed distros, try logging out and logging in again to clear the environment variables.

TODO: Track down how AMENT_PREFIX_PATH is being polluted in this scenario.


Problem:

the following packages/stacks could not have their rosdep keys resolved to system dependencies:
moveit_servo: Cannot locate rosdep definition for [control_toolbox]

Solution: you forgot to run rosdep update


Problem: MoveIt Studio Behavior is always failing

Solution: Launch the Developer RViz configuration and use the MTC plugin to inspect each stage’s solution and why they are failing:

ros2 launch moveit_studio_agent developer_rviz.launch.py

Problem: When using multi-machine setups, the machines’ clocks go out of sync. This can been seen by msgs coming from the future. Or TF complaining about old msgs.

Solution: To solve this, one or both of the machines should install chrony:

sudo apt-get install chrony

If the any machine in the setup does not have access to NTP, the machine that has chrony installed can server as an NTP server. Suppose the drivers RTPC does not have an internet connection, excepting a direct link to the agent PC. First install chrony on the agent PC, then add lines to /etc/chrony/chrony.conf to allow the drivers RTPC to connect to it. For example:

# Allow drivers PC to use the agent PC as NTP server
allow 192.168.1.2  # This should be the IP address of drivers RTPC

Then restart the service:

sudo systemctl restart chrony.service

On the drivers side, reconfigure timesyncd to use the agent PC as an NTP server. Modify /etc/systemd/timesyncd.conf to have:

[Time]
# Use the Agent PC as the primary NTP source
NTP=192.168.1.1  # This should be the IP address of the agent PC

And restart the service:

systemctl restart systemd-timesyncd.service

For more information on more complex installations: Setup guide


Problem: Unsure how scene is modeled

Solution: Debug with the developer RViz configuration:

ros2 launch moveit_studio_agent developer_rviz.launch.py

Problem: MoveIt Studio UI does not finish loading. The robot or other objects are not displayed.

Solution: A previous process likely did not shut down properly. Close the terminals that were previously running Studio with Ctrl+D. Then open new terminals and launch again.


Problem: Robot fails planning after a very short time trying.

Solution: Check that the robot isn’t in a position outside of the specified joint limits. Try to use the teach pendant to set the joints to near zero and see if that fixes things.


Problem:

--- stderr: moveit_studio_behavior_msgs
CMake Error: The source directory "/home/picknik/ws_studio/src/moveit_studio/moveit_studio_behavior_msgs" does not exist.
Specify --help for usage, or press the help button on the CMake GUI.
make: *** [Makefile:6056: cmake_check_build_system] Error 1

Solution: Remove moveit_studio_behavior_msgs folder in both install and build:

rm -r build/moveit_studio_behavior_msgs/ install/moveit_studio_behavior_msgs/

Problem: Can’t Use 2 Realsense video feeds after required driver update.

Solution: Use realsense_ros - v3.2.2, librealsense2 - v2.49.00. To downgrade these packages run install_librealsense.sh in moveit_studio/bin and rebuild your workspace. More information about this issue in PR #619.


Problem: One of the cameras does not come up properly when MoveIt Studio is launched:

[realsense2_camera_node-15] [RealSenseCameraNode]: Device 123456789012 is connected using a 2.1 port. Reduced performance is expected.

Solution: 1. Verify that the cable and USB port are both rated USB 3. 2. Unplug the cable from the camera and the computer and plug it back in. 3. Open the realsense-viewer and perform a hardware reset by clicking “More” and selecting “Hardware Reset”. 4. Try connecting a different cable.


Problem: Formant Client isn’t starting Properly

When you use app.formant.io and the robot isn’t online.

Solution:

Try running the Formant Client manually:

/usr/lib/formant/agent/formant-agent

This should give you an error message for why it’s not starting successfully.


Problem: When running the MoveIt Studio agent, the controller manager is not available and then dies:

[ERROR] [1640282574.233456180] [spawner_streaming_controller]: Controller manager not available
...
[ERROR] [ros2 run controller_manager spawner --stopped joint_trajectory_controller-7]: process has died

Solution:

Remove the following line from your cyclonedds.xml file:

<AllowMulticast>false</AllowMulticast>

Problem: The camera feeds don’t appear in the Web UI, even though they are online and visible via RViz.

Solution:

Try restarting the Formant Agent’s systemd services with systemctl restart formant-agent formant-sidecar on the agent PC. Then re-launch the bridge and the cameras should re-appear.


Problem: When running the MoveIt Studio agent, startup hangs when requesting the list of world names:

[ros_gz_sim]: Requesting list of world names.
[ros_gz_sim]: Requesting list of world names.

Solution:

This can be caused by the default Ubuntu firewall package blocking Ignition’s internal multicast communication. You can work around this by temporarily disabling the firewall:

ufw disable

Problem: The agent PC and the RTPC are connected over the network, but the agent cannot seem to access action servers or services provided by the robot drivers, or discover controllers, resulting in errors such as:

[ERROR] [1658269925.062171689] [MoveGripperAction]: Error code: -25; Message: Failed to send trajectory execution action goal to server.; Details: Action server not available.
...
[ERROR] [1658269931.800549146] [Teleoperate]: Error code: 99999; Message: Failed to enable required controller streaming_controller; Details: Service /ensure_controller_is_active failed with message: Failed to ensure that specified controllers are active.
...
[ERROR] [1658269939.054476603] [moveit_ros.trajectory_execution_manager]: Controller '/joint_trajectory_controller' is not known

Solution:

Check that the network interface specified by the environment variable CYCLONEDDS_NETWORK_INTERFACE matches the network interface specified in ~/.ros/cyclonedds.xml, and that the IP address of the network interface is listed in the environment variable CYCLONEDDS_PEER_ADDRESSES.

Problem: Upon launching the MoveIt Studio agent in simulation mode, the scene and robot are not spawned in Gazebo.

Solution: This could have at least two unrelated causes. First, it could be that these commands failed:

/opt/ros/humble/lib/gazebo_ros/spawn_entity.py -file ${HOME}/.config/moveit_studio/auto_created/simulation_scene.urdf -entity cabinet -x 0.0 -y 0.0 -z 0.0 -Y 0.0 --ros-args
/opt/ros/humble/lib/gazebo_ros/spawn_entity.py -file ${HOME}/.config/moveit_studio/auto_created/robot.urdf -entity robot -x 0.0 -y 0.0 -z 0.0 --ros-args

(Check the terminal output of agent.app.) Try opening a new terminal, and copy-paste the two lines above after waiting a few seconds. If they succeed, then run gui.app. This failure has been observed when running MoveIt Studio inside a virtual machine, where graphics acceleration might be minimal.

If this does not fix the issue, your Cyclone DDS settings might be the cause (even though the error messages are the same as in the previous case). Try replacing ${HOME}/.config/moveit_studio/cyclonedds.xml with ${STUDIO_WS}/src/moveit_studio/bin/templates/cyclonedds_localhost.xml.

Problem: When running Studio agent.app, the controller manager not available and then dies:

[ERROR] [1640282574.233456180] [spawner_streaming_controller]: Controller manager not available
...
[ERROR] [ros2 run controller_manager spawner --stopped joint_trajectory_controller-7]: process has died

Solution:

Remove the following line from your cyclonedds.xml file:

<AllowMulticast>false</AllowMulticast>

Problem: When bringing up MoveIt Studio using the command /.moveit_studio run <moveit_studio_config_name>, you see the following error ::

dependency failed to start: container moveit_studio-<container-name>-1 exited (0)
Waiting for MoveIt Studio to start...
Waiting for MoveIt Studio to start...
Waiting for MoveIt Studio to start...
Waiting for MoveIt Studio to start...
Waiting for MoveIt Studio to start...
Waiting for MoveIt Studio to start...
Waiting for MoveIt Studio to start...
Waiting for MoveIt Studio to start...
Waiting for MoveIt Studio to start...

Timed out waiting for MoveIt Studio containers.

Solution:

You can get more information on why MoveIt Studio containers failed to start by running /.moveit_studio -v run <moveit_studio_config_name>.

The run script takes 30 seconds to confirm if the MoveIt Studio containers are down. You will be able to run other commands once you see the message Timed out waiting for MoveIt Studio containers.

Problem: The robot arm got into collision while using endpoint/joint control and the robot cannot be moved and Objectives cannot be executed henceforth.

Solution:

This would happen if the rate of moving the arm is greater than the rate of collision checking. To increase the rate of collision checking, the following parameters can be tweaked in servo.yaml::

collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.

Restart MoveIt Studio as that will reset the robot’s position in sim. The initial position can be set in initial_position.yaml in the robot config package.