Ros2 plugin

[ravnicas]

Changed kinematics plugin to match kdl CMakeFiles and classes.
Can be loaded via: kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin.

Renamed kinematic_plugin folder to to kinematics, so it more closely matches moveits repo.

Tested on ros2 galactic.

One has to download nlopt and build it by hand though.

[aprotyas]

@ravnicas thanks a lot for the contribution! Before I actually review it, I'm going to let CI run - just to see if it complains about anything (other than linters). I will be reviewing the PR shortly after CI has run.

[aprotyas]

I took the liberty to update references to the old package name trac_ik_kinematics_plugin to the new package name trac_ik_kinematics (I have no objections to this change by the way) in f624c01. The plugin should now actually be built/tested in CI.

[ravnicas]

If I get the CI correctly, it doesnt like the dependency on moveit packages. e.g moveit_ros_planning.

Also if i got it right, they are needed so i can use the abstract classes from kinematics_base and so on. I didnt want to work arond the dependencies, since one would use moveit anyway if one wants to use the plugin.

I took the liberty to update references to the old package name trac_ik_kinematics_plugin to the new package name trac_ik_kinematics (I have no objections to this change by the way) in f624c01. The plugin should now actually be built/tested in CI.

[dpbnasika]

Hi, thanks for porting the package, i took the packages trac_ik_kinematics and trac_ik_lib and pasted in my colcon workspace and built all together with my robot's moveit package. i changed the kinematics.yaml file, but after that i see no goal state interactive marker on my rviz, do you have a bit of documentation on how to use it?. it would be helpful.

thank you

[ravnicas]

What version of ROS are you using it on? I had some problems seeing the interactive marker as well, but i'm also commanding the robots via console. The interactive marker does work on version 2.4.0 from the master repository of moveit.

[dpbnasika]

@ravnicas i use ros foxy. but when i try to use it via console, i get an error

[ERROR] [1656591386.436187807] [kinematics_plugin_loader]: The kinematics plugin (arm) failed to load. Error: Failed to load library libmoveit_trac_ik_kinematics_plugin.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library LoadLibrary error: libnlopt.so.0: cannot open shared object file: No such file or directory, at /tmp/binarydeb/ros-foxy-rcutils-1.1.3/src/shared_library.c:84
[move_goal_node-1] [ERROR] [1656591386.436227947] [moveit_ros.robot_model_loader]: Kinematics solver could not be instantiated for joint group arm.

Best Regards
Dp

[ravnicas]

It can't find nlopt, afaik you can install it from their homepage or as package.

https://nlopt.readthedocs.io/en/latest/

This is what I have installed, since the other nlopt package from ros wasn't ported.

[dpbnasika]

@ravnicas i did intsall that, without that both the packages wont compile, and i can build them without an error. so my question is how to configure the plugin into the moveit package?. my structure is like this,

colcon_ws

• src
• package1
• moveit package
• trac_ik_lib
• trac_ik_kinematics
• package2 with hardware interface

now do i need to change anything in CMakeLists.txt or Package.xml of moveit package for trac_ik_kinematics_plugin to work ?.

[ravnicas]

no you don't need to change anything, just make sure your ros installation and also the moveit installation is sourced correctly

[dpbnasika]

@ravnicas they are sourced properly, but still it does not work. i dont know why.

[ravnicas]

it should look like this:

colcon_ws/src/trac_ik/
├── trac_ik
├── trac_ik_examples
├── trac_ik_kinematics
├── trac_ik_lib
└── trac_ik_python

also:
colcon_ws/src/
├── your_pkg_1
└── your_pkg_whatever

[dpbnasika]

@ravnicas i did git clone -b ros2_plugin https://github.com/ravnicas/trac_ik/tree/ros2_plugin in my src folders and ofcourse it is not building, because i see there is no CmakeLists.txt or package.xml directly under trac_ik package as you mentioned. How could you colcon build it?.

edit: that is why i compiled trac_ik_kinematics and trac_ik_lib packages seperately and put them next to other packages in my src folder.

[ravnicas]

well, i cannot reproduce your error in any configuration, im sorry

also cloning the whole folder to your src directory of your ws does indeed compile

[G-Yong]

when I run “colcon build”，I got these：
...
/home/yong/Desktop/trac_ik/src/trac_ik-ros2_plugin/trac_ik_kinematics/trac_ik_kinematics_plugin/src/trac_ik_kinematics_plugin.cpp:387:30: error: ‘const IKCallbackFn’ {aka ‘const class std::function<void(const geometry_msgs::msg::Pose_<std::allocator >&, const std::vector<double, std::allocator >&, moveit_msgs::msg::MoveItErrorCodes_<std::allocator >&)>’} has no member named ‘empty’; did you mean ‘_M_empty’?
387 | if (!solution_callback.empty())
| ^~~~~
| _M_empty
...
It seems that std::function has no member named 'empty'. How can I solve this problem?

[aprotyas]

when I run “colcon”，I got these： ... /home/yong/Desktop/trac_ik/src/trac_ik-ros2_plugin/trac_ik_kinematics/trac_ik_kinematics_plugin/src/trac_ik_kinematics_plugin.cpp:387:30: error: ‘const IKCallbackFn’ {aka ‘const class std::function<void(const geometry_msgs::msg::Pose_<std::allocator >&, const std::vector<double, std::allocator >&, moveit_msgs::msg::MoveItErrorCodes_<std::allocator >&)>’} has no member named ‘empty’; did you mean ‘_M_empty’? 387 | if (!solution_callback.empty()) | ^~~~~ | _M_empty ... It seems that std::function has no member named 'empty'. How can I solve this problem?

@G-Yong this PR is not ready to land, so you won't be able to compile it as-is. Sorry!

As for this specific issue -- kinematics::KinematicsBase::IKCallbackFn is now a type alias for std::function<>. Previously, it used to be a type alias for boost::function<>. So, previously, the IKCallbackFn::empty() method would return a boolean value indicating whether the function object was empty or not.

Since IKCallbackFn now type aliases std::function<> instead, the workaround here is to replace IKCallbackFn::empty() calls with IKCallbackFn::operator bool() calls as so:

- if (!solution_callback.empty())
+ if (solution_callback)

---

Unfortunately, I don't have the bandwidth to actually finish this work. If you want to take a drive-by shot at it, I'd be happy to do brief code reviews!