ROS 2 port & Gazebo demos

[AndrejOrsula]

This PR ports moveit_calibration to ROS 2. It is a direct continuation of https://github.com/ros-planning/moveit_calibration/pull/101 by @Abishalini and @vatanaksoytezer.

Besides the progress from last year:

• I integrated hand-eye solvers from OpenCV to provide 2 extra methods and improve maintainability (it no longer depends on crigroup/handeye, crigroup/criutils and crigroup/baldor).
• I added Gazebo demos (with Docker setup) to test the implementation and to extend moveit2_tutorials in this PR.
• I added exporters of the supported ROS 2 launch formats (Python, XML and YAML) that run a tf2_ros/static_transform_publisher node using the estimated extrinsics.

Two of the Gazebo demos can be seen below:

https://github.com/ros-planning/moveit_calibration/assets/22929099/b6194d07-2bbb-405a-b0ea-9b1c643f8d22

Note: I tried the official Gazebo config under moveit2_tutorials, but I couldn't get it to work for the demos. I had to use my own Panda config (https://github.com/AndrejOrsula/panda_ign_moveit2) in order to support Gazebo/Ignition plugins required for the demos (requires launch scripts that export SDF before starting Gazebo, and some plugins must be defined under the SDF of the model such as the DetachableJoint).

Tested on ROS 2 humble & Gazebo fortress.

• The Panda setup I used does not support iron/rolling yet (waiting for gz_ros2_control).
• Same thing with garden. It should technically work, but there were some issues compiling gz_ros2_control when I tried it.

The moveit_calibration_demos/.docker directory currently provides some custom helper scripts. I can see in several MoveIt 2 repositories that extensively use docker-compose instead. Please, let me know if you want to change it to docker-compose (I might need some help with GUI and GPU, though).

Try it out! With this one-liner, you can test the Gazebo demos yourself inside a single Docker container! It automatically pulls the pre-built image from Docker Hub and runs it with the required flags (tested on Ubuntu 22.04 with NVIDIA GPU, but rendering might NOT be functional out-of-the-box on some machines).

bash -c "$(wget -qO - https://raw.githubusercontent.com/AndrejOrsula/moveit2_calibration/ros2_port/moveit_calibration_demos/.docker/run.bash)" -- ros2 launch moveit_calibration_demos gz_eye_in_hand_charuco.launch.py

Note: If you are against curl | sh / wget | sh syntax, you can first inspect the script here.

There are 3 additional demos that you can try for eye-in-hand & eye-to-hand using ChArUco or ArUco patterns (no setup required, just change the command).

• ros2 launch moveit_calibration_demos gz_eye_to_hand_charuco.launch.py
• ros2 launch moveit_calibration_demos gz_eye_in_hand_aruco.launch.py
• ros2 launch moveit_calibration_demos gz_eye_to_hand_aruco.launch.py

6 June 2022: Original PR description (Click to expand)

Below is a short video to illustrate the current state of this PR (6 June 2022). https://user-images.githubusercontent.com/22929099/172181846-48e7f002-77ba-4196-b0b9-110d569a0488.mp4 #### Missing features - [`HandEyeSolverDefault::solve()`](https://github.com/AndrejOrsula/moveit2_calibration/blob/b8f2408c8fa7e63063d8eb79f728abec6e1c672a/moveit_calibration_plugins/handeye_calibration_solver/src/handeye_solver_default.cpp#L64-L66) needs to be ported to ROS 2. This function relies on Python module implemented in [crigroup/handeye](https://github.com/crigroup/handeye) ROS package. **To finalise this PR, [crigroup/handeye](https://github.com/crigroup/handeye) needs to be ported to ROS 2 (or some alternative `solve()` method needs to be implemented).** - Package [crigroup/handeye](https://github.com/crigroup/handeye) requires [crigroup/criutils](https://github.com/crigroup/criutils) and [crigroup/baldor](https://github.com/crigroup/baldor) to be ported as well. - I believe that @JStech already started porting these to ROS 2 via https://github.com/crigroup/criutils/pull/5 and https://github.com/crigroup/baldor/pull/5. - Issue that *kinda* tracks port of [crigroup/handeye](https://github.com/crigroup/handeye) to ROS 2: https://github.com/crigroup/handeye/issues/5 - Tests need to be tested and fixed. Otherwise, everything else that is essential *seems* to be functional. Most GUI elements were checked, but anything that requires the calibration to be "solved" was not tested for obvious reasons.

---

Note to maintainer(s): Please, change the target of this PR if desired. Note to reviewer(s): I had difficulties getting pluginlib classes to be exported correctly at the beginning, so I ended up rewriting CMake config. Please, check that everything is MoveIt compatible (commit=https://github.com/ros-planning/moveit_calibration/commit/1a814450b873769edb0a30188f80cc1510954aec).

[vatanaksoytezer]

@AndrejOrsula thank you so much for working on this. The progress looks awesome! I'll try to add a ROS 2 CI job to this, fork and complete ports of necessary packages of crigroup. The project seems to be not updated for some time and they seem a bit unresponsive from the PRs you linked, so I'm assuming it would be OK to fork and port it. I think we can figure out about the releases later.

[ErikOrjehag]

What's the status of this? :) Was planning to use moveit_calibration in my project. Anything I can help with? @Abishalini @vatanaksoytezer @AndrejOrsula

[AndrejOrsula]

Hello @ErikOrjehag,

Thank you for commenting. I do not believe there has been any update on this. Personally, I have not looked into it any further so far.

As the next (and maybe final) step, HandEyeSolverDefault::solve() needs to be ported to ROS 2. Other functionalities are ported to ROS 2 already, but they are not fully tested yet... For more details, see the PR description. The included video should give an idea about the current state.

[ErikOrjehag]

@AndrejOrsula Hi! Thanks for the response. I forked @Abishalini latest work here https://github.com/DynoRobotics/moveit_calibration with this commit that I needed for it to build.

The package builds but the panel is not showing up in rviz2 under Panels > Add new panel.

It seems to have exported the pluginlib?

➜  ws git:(main) ✗ find /moveit_ws/install -name "*rviz_common__pluginlib__plugin*"
/moveit_ws/install/moveit_task_constructor_visualization/share/ament_index/resource_index/rviz_common__pluginlib__plugin
/moveit_ws/install/moveit_ros_visualization/share/ament_index/resource_index/rviz_common__pluginlib__plugin
➜  ws git:(main) ✗ 
➜  ws git:(main) ✗ find install -name "*rviz_common__pluginlib__plugin*"           
install/moveit_calibration_gui/share/ament_index/resource_index/rviz_common__pluginlib__plugin
➜  ws git:(main) ✗ 
➜  ws git:(main) ✗ 

But rviz does not seem to find it?

➜  ws git:(main) ✗ rviz2 --plugin moveit_rviz_plugin/HandEyeCalibration                                     
QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-ros'

No such plugin for spec "moveit_rviz_plugin/HandEyeCalibration"

[INFO] [1669199828.144924199] [rviz2]: Stereo is NOT SUPPORTED
[INFO] [1669199828.145068497] [rviz2]: OpenGl version: 4.5 (GLSL 4.5)
[INFO] [1669199828.166738314] [rviz2]: Stereo is NOT SUPPORTED

Do you have any tips on how I could continue investigating? Some file I should look for or debug prints that I can enable in rviz?

[AndrejOrsula]

Hello @ErikOrjehag,

Unfortunately, I am not exactly sure what could be wrong as I have not tested the PR for quite some time. I can only provide a few pointers, but I am not sure if it will be of any help (I don't currently have time to check/test it).

• As a sanity check, make sure you source the workspace where you built the moveit_calibration_plugins/moveit_calibration_gui packages before listing plugins & running RViz.
• I built the workspace of the package with colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON (but I don't expect these CMake args to make any difference).
• What ROS 2 distribution are you using? I was using humble when testing this (sync/release around May/June 2022). Maybe there are some significant changes of pluginlib in the current humble or rolling compared to the release I tested the PR with? Maybe something in the release notes?

[ErikOrjehag]

@AndrejOrsula Thank you :) It turns out it was only a silly mistake. The ros1 docs says to use Panel > Add new panel but the ros2 port is not a panel but a display so I found it under Displays > Add > By display type.

Now I'm stuck on this error message instead:

IMPORTANT: PLEASE READ THIS FOR ADVICE ON HOW TO SOLVE THIS ISSUE!

Importing the numpy C-extensions failed. This error can happen for
many reasons, often due to issues with your setup or how NumPy was
installed.

We have compiled some common reasons and troubleshooting tips at:

    https://numpy.org/devdocs/user/troubleshooting-importerror.html

Please note and check the following:

  * The Python version is: Python3.10 from ""
  * The NumPy version is: "1.21.5"

and make sure that they are the versions you expect.
Please carefully study the documentation linked above for further help.

Original error was: /usr/lib/python3/dist-packages/numpy/core/_multiarray_umath.cpython-310-x86_64-linux-gnu.so: undefined symbol: PyExc_RecursionError

➜  ws git:(main) ✗ ldd -d /usr/lib/python3/dist-packages/numpy/core/_multiarray_umath.cpython-310-x86_64-linux-gnu.so | grep PyExc_RecursionError
undefined symbol: PyExc_RecursionError  (/usr/lib/python3/dist-packages/numpy/core/_multiarray_umath.cpython-310-x86_64-linux-gnu.so)

➜  ws git:(main) ✗ readelf -s install/moveit_calibration_plugins/lib/libmoveit_handeye_calibration_solver_core.so | grep PyExc               
    21: 0000000000000000     0 OBJECT  GLOBAL DEFAULT  UND PyExc_AttributeError
    60: 0000000000000000     0 OBJECT  GLOBAL DEFAULT  UND PyExc_RuntimeError
    80: 0000000000000000     0 OBJECT  GLOBAL DEFAULT  UND PyExc_ImportError
    84: 0000000000000000     0 OBJECT  GLOBAL DEFAULT  UND PyExc_AttributeError
   134: 0000000000000000     0 OBJECT  GLOBAL DEFAULT  UND PyExc_RuntimeError
   162: 0000000000000000     0 OBJECT  GLOBAL DEFAULT  UND PyExc_ImportError

➜  ws git:(main) ✗ readelf -s /usr/bin/python3 | grep PyExc_RecursionError                                                              
   950: 00000000005828b8     8 OBJECT  GLOBAL DEFAULT   27 PyExc_RecursionError

I will continue trouble shooting...

[ErikOrjehag]

Adding this line solved it:

dlopen("/usr/lib/x86_64-linux-gnu/libpython3.10.so.1.0", RTLD_LAZY | RTLD_GLOBAL);

Not a very elegant solution, I'm guessing the correct way would be to change something in the CMakeLists?

At least now I'm up to speed with similar results as in your video:

@AndrejOrsula How come you stopped porting at this step? Any particular pitfalls I should look out for?

[ErikOrjehag]

I did a kindy janky humble port of handeye as a ament_python package without the handeye_server and service messages because it was enough to get movit_calibration working. Also ported baldor to humble. Skipped criutils because it was not used in the files ran by movit_calibration. Was able to get some values out from clicking "solve" in the GUI before I left the office yesterday. Will experiment more today to see if I can use the calibration and that it is correct :)

[AndrejOrsula]

Adding this line solved it:

dlopen("/usr/lib/x86_64-linux-gnu/libpython3.10.so.1.0", RTLD_LAZY | RTLD_GLOBAL);

Not a very elegant solution, I'm guessing the correct way would be to change something in the CMakeLists?

I am glad you were able to resolve the issue but I am unsure why it would be needed. Something might be missing but the current CMake setup for the Python wrapper worked for me when I tried it the last time.

@AndrejOrsula How come you stopped porting at this step? Any particular pitfalls I should look out for?

I looked into porting moveit_calibration to ROS 2 during the last World MoveIt Day. I only spent that day on it and I did not have a chance to revisit it. The current state of the PR seemed like a good stopping point when I discovered that the solver uses Python-based dependencies that were not ported to ROS 2 yet. The next step would therefore be to port these dependencies (or integrate some other alternative solvers that are maintained). No particular pitfalls that I remember -- just some GUI stuff that might need to be polished, e.g. the size of the dropdown menu text.

I did a kindy janky humble port of handeye as a ament_python package without the handeye_server and service messages because it was enough to get movit_calibration working. Also ported baldor to humble. Skipped criutils because it was not used in the files ran by movit_calibration. Was able to get some values out from clicking "solve" in the GUI before I left the office yesterday. Will experiment more today to see if I can use the calibration and that it is correct :)

Good job! Looking forward to hear more.

[ErikOrjehag]

@AndrejOrsula Got it working, here I'm projecting a point in front of the robot back into the camera.

I did however need to modify the output roll, pitch, yaw values from the calibration like this:

  <!-- Camera -->
  <link name="camera_calibrated">
  </link>

  <!-- values copied from the gui of moveit_calibration -->
  <xacro:property name="c_roll" value="3.1302"/>
  <xacro:property name="c_pitch" value="-3.0507"/>
  <xacro:property name="c_yaw" value="-1.5320"/>

  <xacro:property name="c_x" value="0.0614"/>
  <xacro:property name="c_y" value="0.0000"/>
  <xacro:property name="c_z" value="0.0550"/>

  <joint name="camera_calibrated_joint" type="fixed">
    <origin
            xyz="${c_x} ${c_y} ${c_z}"
            <!-- NOTICE: that roll and pitch are switched with each other and
                                   I need to negate the pitch and add 2 PI -->
            rpy="${2*pi - c_pitch} ${c_roll} ${c_yaw}" />
    <parent link="end_effector" />
    <child link="camera_calibrated" />
  </joint>

[lukicdarkoo]

@AndrejOrsula Any suggestion on how to proceed on this? It seems @crigroup doesn't really intend to support the Python packages anymore. I guess finding an alternative may be a better approach.

[AndrejOrsula]

@AndrejOrsula Any suggestion on how to proceed on this? It seems @crigroup doesn't really intend to support the Python packages anymore. I guess finding an alternative may be a better approach.

Hello @lukicdarkoo,

I am not aware of the current status of crigroup/handeye, nor any plans for "officially" porting this package to ROS 2 and its future maintenance. As discussed above, @ErikOrjehag was able to create a functional port to humble in their fork, so it could be used as a starting point if someone wanted to continue with that.

As for finding an alternative solver that is actively maintained, it might indeed be a better approach. For that, moveit/moveit_calibration maintainers would first need to approve going in this direction, as it would introduce a significant change between ROS 1 and ROS 2 versions of moveit_calibration.

My personal opinion is that OpenCV's calib3d module could be suitable as a valid alternative. OpenCV is already utilised as a dependency of moveit_calibration (including this ROS 2 port). Relevant public API is documented, it supports several methods for hand-eye calibration, and it includes tests (and possibly other examples) that can be used as a starting point. Going this route would also avoid interfacing with Python, unlike the current approach. However, I have not investigated it further so I don't know if there are any functionalities/features that would be missing to achieve a feature parity with the current ROS 1 version.

Surely, there are also other alternatives that could potentially be more suitable. Please, let us know if you have some suggestions. :smiley:

[JStech]

The OpenCV solvers are definitely a good path forward--I was working on that previously, and you can see my old branch here, https://github.com/ros-planning/moveit_calibration/tree/opencv-solvers. When MoveIt Calibration was originally written, these solvers didn't exist.

I also remember working on porting the crigroup packages to ROS 2, although I don't know that I was able to get them all ported.

I've neglected this package since I left PickNik. I'm glad there's still interest in getting it moved to ROS 2.

[akhild5555]

@ErikOrjehag Following you work here I was able to get up to the "Error: Failed to load python module: handeye.calibrator" error. How were you able to get the calibration working beyond this point? I've built your humble port of handeye but I still get that error when I hit "Solve" in the calibration plugin. It seems like the moveit_calibration package is unable to find the handeye build files?

I would really appreciate it if you could provide a bit more detail! Thank you!

[akhild5555]

Actually I was able to resolve this today with debugging help from chatGPT. The problem was I did not clone and build the humble port of the baldor package made by @ErikOrjehag and then I also pip installed scipy. Then I was able to solve the calibration after obtaining samples!

[AndrejOrsula]

It has been a whole year since opening this PR during the previous World MoveIt Day. I think it is about time to finalize it. :sweat_smile:

If nobody minds, I will work towards finishing it during this year's WMD. As discussed with @JStech in the comments above, I would replace the current solvers from crigroup/handeye with the OpenCV solvers to improve maintainability. I won't have access to a real robot this Thursday, so I will also add a demo package with a simulated Gazebo world.

[AndrejOrsula]

This PR is now ready for review. I updated the PR description.

There are certainly improvements to the code that could be made, but I think it is in a good state to merge and then open new PRs to address tests, optimization and clean-up.

[AndrejOrsula]

Thank you so much @JStech for taking a look at it!

One thing I forgot to mention is that the Gazebo demos are now configured with an RGB-D camera. The depth is currently unused, but I think it would be cool if this package eventually supported hand-eye calibration also in unstructured environments (without fiducial markers/known patterns). With RGB-D cameras being so popular and affordable now, finding a set of stable keypoints and determining their combined 3D pose via depth map instead of PnP might be a nice-and-quick alternative for certain use cases.

[sgarciav]

Hello all! Thanks @AndrejOrsula for working on this port.

I gave it a try using ROS 2 Humble and a Oak-D camera from Luxonis. The first thing I noted was that the camera intrinsics were not being set correctly. I found out that only the first camera info message received was being evaluated. If setCameraIntrinsicParams returns false, the camera_info_ data is updated anyways.

This PR updates the logic such that the camera_info_ variable is updated only when the camera intrinsics are set correctly.

[sgarciav]

Another update for your consideration, is adding support for the rational polynomial distortion model. I've made the changes in my fork. Should I create a PR for this update? If so, should it be to @Abishalini's repo?

Any comments/suggestions are more than welcomed. Thanks!

Credit goes to this PR that was merged for ROS 1.

[Danilrivero]

Hello there,

First of all, thanks for all the work put into this, this tool in ROS2 will prove useful for many. Just wondering if there were any plans to merge this PR since there were other proposals made by other forks such as this fork from @sgarciav.

This PR is now ready for review. I updated the PR description.

There are certainly improvements to the code that could be made, but I think it is in a good state to merge and then open new PRs to address tests, optimization and clean-up.

Seems like there a few PRs related to a ROS2 port now with different features. It could be nice to address new improvements in different PRs as stated and have a stable branch for ROS2 with this port by now.

Warm regards, thanks to everyone.