Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ROS 2 port & Gazebo demos #118

Open
wants to merge 34 commits into
base: ros2
Choose a base branch
from
Open

Conversation

AndrejOrsula
Copy link

@AndrejOrsula AndrejOrsula commented Jun 6, 2022

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

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

Abishalini and others added 24 commits August 4, 2021 13:41
Fix tf visual tools, add image transport dependency
Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
@vatanaksoytezer
Copy link

@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
Copy link

ErikOrjehag commented Nov 21, 2022

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

@AndrejOrsula
Copy link
Author

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
Copy link

@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

Screenshot from 2022-11-23 11-38-33

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
Copy link
Author

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
Copy link

ErikOrjehag commented Nov 23, 2022

@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

Screenshot from 2022-11-23 14-40-26

➜  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
Copy link

Adding this line solved it:

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

Screenshot from 2022-11-23 15-05-36

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:
Screenshot from 2022-11-23 15-04-28

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

@ErikOrjehag
Copy link

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
Copy link
Author

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
Copy link

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

project-into-image-optim

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
Copy link

@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
Copy link
Author

@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
Copy link
Contributor

JStech commented Mar 29, 2023

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
Copy link

akhild5555 commented Apr 11, 2023

@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
Copy link

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
Copy link
Author

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>
Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
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 AndrejOrsula marked this pull request as ready for review June 8, 2023 19:54
@AndrejOrsula AndrejOrsula changed the title ROS 2 port (continuation) ROS 2 port & Gazebo demos Jun 8, 2023
Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
@AndrejOrsula
Copy link
Author

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.

Copy link
Contributor

@JStech JStech left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think .yml is also common.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Added in 6712423

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This could be replaced with a map.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, that's much cleaner. ☺️

Refactored in 0263c8c

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
@AndrejOrsula
Copy link
Author

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

7 participants