ROS 2 port & Gazebo demos #118

AndrejOrsula · 2022-06-06T15:00:53Z

This PR ports moveit_calibration to ROS 2. It is a direct continuation of #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:

moveit2_calibration.mp4

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).

moveit_calibration_ros2_wip_demo.mp4

Missing features

HandEyeSolverDefault::solve() needs to be ported to ROS 2. This function relies on Python module implemented in crigroup/handeye ROS package. To finalise this PR, crigroup/handeye needs to be ported to ROS 2 (or some alternative solve() method needs to be implemented).
- Package crigroup/handeye requires crigroup/criutils and crigroup/baldor to be ported as well.
  - I believe that @JStech already started porting these to ROS 2 via Foxy port crigroup/criutils#5 and ROS2 Foxy port crigroup/baldor#5.
- Issue that kinda tracks port of crigroup/handeye to ROS 2: Release to Noetic and ROS2 crigroup/handeye#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=1a81445).

Fix tf visual tools, add image transport dependency

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

vatanaksoytezer · 2022-06-06T15:21:30Z

@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 · 2022-11-21T13:08:46Z

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

AndrejOrsula · 2022-11-21T13:32:46Z

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 · 2022-11-23T14:08:08Z

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 · 2022-11-24T07:33:14Z

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 · 2022-11-29T21:49:30Z

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 · 2022-12-08T10:51:37Z

@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 · 2023-03-28T14:38:13Z

@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 · 2023-03-29T12:16:21Z

@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. 😃

JStech · 2023-03-29T13:04:09Z

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 · 2023-04-11T21:23:51Z

@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 · 2023-04-12T22:08:29Z

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 · 2023-06-06T18:00:59Z

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. 😅

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.

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

- This replaces ROS 1 solvers from crigroup Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

AndrejOrsula · 2023-06-08T22:03:45Z

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.

JStech

Thanks for all your work on this! I left a few suggestions, but none of them are blockers. LGTM 👍

JStech · 2023-06-09T10:36:19Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp

+    ss << "              # --roll " << r_euler[0] << std::endl;
+    ss << "              # --pitch " << r_euler[1] << std::endl;
+    ss << "              # --yaw " << r_euler[2] << std::endl;
+  }


I think it'd be cleaner to put each of these blocks in a separate function: outputCalibrationPy, outputCalibrationXml, outputCalibrationYaml.

True! Thank you. Refactored in 23dd00b

I think there are several places like this where code style could be improved. However, I would rather do it after tests work so I don't accidentally break something.

JStech · 2023-06-09T10:36:42Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp

+    ss << "    -->" << std::endl;
+    ss << "</launch>" << std::endl;
+  }
+  else if (file_name.endsWith(".yaml"))


I think .yml is also common.

I was not aware of that in the context of ROS 2 launch files, but it makes sense. Thank you!

Added in 6712423

JStech · 2023-06-09T10:46:58Z

moveit_calibration_plugins/handeye_calibration_solver/src/handeye_solver_opencv.cpp

+    *error_message = "Unsupported handeye solver: " + solver_name;
+    RCLCPP_ERROR_STREAM(LOGGER_CALIBRATION_SOLVER, *error_message);
+    return false;
+  }


This could be replaced with a map.

Yes, that's much cleaner. ☺️

Refactored in 0263c8c

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

AndrejOrsula · 2023-06-09T12:48:48Z

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 · 2024-05-20T21:03:02Z

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 · 2024-05-20T21:05:56Z

...s/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h

    }
  };

-  const std::string LOGNAME = "handeye_target_base";
+  rclcpp::Clock clock;


This clock variable is not being used.

sgarciav · 2024-05-21T13:49:40Z

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 · 2024-07-24T12:32:19Z

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.

Abishalini and others added 24 commits August 4, 2021 13:41

Inital port of handeye_calibration_target

2e6b0af

handeye_calibration_target compiles

47b88ca

Initial port of handeye_calibration_solver

8384400

Initial port of moveit_calibration_gui

1c764fa

Fix CMakeLists for moveit_calibration_gui

1c63c60

Port headers in moveit_calibration_gui

e78726c

Initial port of source files

ed75892

Some more changes

772e87d

Some more fixes

7ba71c5

Remove LOGNAME

7a99b24

moveit_calibration_gui compiles without errors

f36358d

Initial port of tests

bcd3b4d

Loads display in RViz

a6cca4d

Add image transport dependency

668929b

Add node for tf_visual_tools

3f70f54

Merge pull request #1 from vatanaksoytezer/pr-fix_ros2

8b1d0c0

Fix tf visual tools, add image transport dependency

Refactor CMake config and fix exporting of pluginlib classes

1a81445

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Fix compilation of tests (not yet functional)

968a750

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Fix compilation of handeye_calibration_target as a shared library

e91de73

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Use a unique rclcpp::logger in handeye_calibration_solver lib

1f64994

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Fix moveit_calibration_gui for ROS 2

76727a2

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Update CHANGELOGs

294ec4c

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Update README with instructions

b1d24cb

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Run pre-commit on all files

b8f2408

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

AndrejOrsula mentioned this pull request Jun 6, 2022

Port Hand-Eye Calibration to ROS 2 and add Gazebo calibration demo moveit/moveit2_tutorials#419

Draft

2 tasks

henningkayser requested a review from Abishalini June 7, 2022 14:22

Works on rolling

010310f

AndrejOrsula added 5 commits June 8, 2023 21:40

Update pre-commit config

d1b6c83

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Add Gazebo demos (moveit_calibration_demos)

15d4cb7

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Add Docker setup for moveit_calibration_demos

90f84fe

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Integrate OpenCV solvers for hand-eye calibration

dd835a7

- This replaces ROS 1 solvers from crigroup Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Add exporters of ROS 2 launch files (Python, XML and YAML)

13d3548

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

AndrejOrsula marked this pull request as ready for review June 8, 2023 19:54

AndrejOrsula changed the title ~~ROS 2 port (continuation)~~ ROS 2 port & Gazebo demos Jun 8, 2023

Add Docker helper script .docker/run.bash

f959c5d

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

AndrejOrsula mentioned this pull request Jun 8, 2023

Changes to get moveit_calibration working on Rolling AndrejOrsula/moveit2_calibration#1

Merged

JStech approved these changes Jun 9, 2023

View reviewed changes

AndrejOrsula added 3 commits June 9, 2023 14:33

Support YAML launch files with .yml extension

6712423

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Refactor launch file exporters into separate functions

23dd00b

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Refactor solver method maching into a map

0263c8c

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

sgarciav reviewed May 20, 2024

View reviewed changes

Danilrivero mentioned this pull request Nov 11, 2024

MoveIt Hand Eye Callibration Package for ROS2 HUMBLE moveit/moveit2#3054

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

ROS 2 port & Gazebo demos #118

ROS 2 port & Gazebo demos #118

AndrejOrsula commented Jun 6, 2022 •

edited

Loading

vatanaksoytezer commented Jun 6, 2022

ErikOrjehag commented Nov 21, 2022 •

edited

Loading

AndrejOrsula commented Nov 21, 2022

ErikOrjehag commented Nov 23, 2022

ErikOrjehag commented Nov 24, 2022

AndrejOrsula commented Nov 29, 2022

ErikOrjehag commented Dec 8, 2022

lukicdarkoo commented Mar 28, 2023

AndrejOrsula commented Mar 29, 2023

JStech commented Mar 29, 2023

akhild5555 commented Apr 11, 2023 •

edited

Loading

akhild5555 commented Apr 12, 2023

AndrejOrsula commented Jun 6, 2023

AndrejOrsula commented Jun 8, 2023

JStech left a comment

JStech Jun 9, 2023

AndrejOrsula Jun 9, 2023

JStech Jun 9, 2023

AndrejOrsula Jun 9, 2023

JStech Jun 9, 2023

AndrejOrsula Jun 9, 2023

AndrejOrsula commented Jun 9, 2023

sgarciav commented May 20, 2024 •

edited

Loading

sgarciav May 20, 2024

sgarciav commented May 21, 2024

Danilrivero commented Jul 24, 2024

ROS 2 port & Gazebo demos #118

Are you sure you want to change the base?

ROS 2 port & Gazebo demos #118

Conversation

AndrejOrsula commented Jun 6, 2022 • edited Loading

Missing features

vatanaksoytezer commented Jun 6, 2022

ErikOrjehag commented Nov 21, 2022 • edited Loading

AndrejOrsula commented Nov 21, 2022

ErikOrjehag commented Nov 23, 2022

ErikOrjehag commented Nov 24, 2022

AndrejOrsula commented Nov 29, 2022

ErikOrjehag commented Dec 8, 2022

lukicdarkoo commented Mar 28, 2023

AndrejOrsula commented Mar 29, 2023

JStech commented Mar 29, 2023

akhild5555 commented Apr 11, 2023 • edited Loading

akhild5555 commented Apr 12, 2023

AndrejOrsula commented Jun 6, 2023

AndrejOrsula commented Jun 8, 2023

JStech left a comment

Choose a reason for hiding this comment

JStech Jun 9, 2023

Choose a reason for hiding this comment

AndrejOrsula Jun 9, 2023

Choose a reason for hiding this comment

JStech Jun 9, 2023

Choose a reason for hiding this comment

AndrejOrsula Jun 9, 2023

Choose a reason for hiding this comment

JStech Jun 9, 2023

Choose a reason for hiding this comment

AndrejOrsula Jun 9, 2023

Choose a reason for hiding this comment

AndrejOrsula commented Jun 9, 2023

sgarciav commented May 20, 2024 • edited Loading

sgarciav May 20, 2024

Choose a reason for hiding this comment

sgarciav commented May 21, 2024

Danilrivero commented Jul 24, 2024

AndrejOrsula commented Jun 6, 2022 •

edited

Loading

ErikOrjehag commented Nov 21, 2022 •

edited

Loading

akhild5555 commented Apr 11, 2023 •

edited

Loading

sgarciav commented May 20, 2024 •

edited

Loading