feat(franka_gazebo): implement set_franka_model_configuration service

[rickstaa]

This PR implements a reset_joint_states service. This service can reset the joint states when pushed outside the joint limits. This can, for example, happen when Gazebo's set_model_configuration is used (see #225 for more information).

Update

This PR implements a set_franka_model_configuration, which can be used to set call Gazebo's set_model_configuration service while ensuring that the reported joint positions stay within joint limits (see #225 for more information)

[gollth]

Good idea @rickstaa! Makes total sense to have something like this in franka_gazebo.

[rickstaa]

@gollth I just forced pushed a new version onto this pull request that implements the set_franka_model_configuration service instead. Please let me know what you think.

Some parts can be improved like:

• Using a mutex lock instead of the requestedPosition_ variable to prevent racing conditions.
• Using one the model_configuration map from the beginning of the service and modifying it along the way.
• Break the set_franka_model_configuration service up in smaller submethods.
• Change the login behaviour.
• Use newer C++ features to clean up the code. The current implementation is based on c11.

However, I left these decisions up to you since they depend on the code style you want to use.

[rickstaa]

@gollth I just saw that I use clamp which was included in C17. I will add a replacement for this.

[rickstaa]

@gollth Clang seems to fail but does not throw a warning about what goes wrong https://github.com/frankaemika/franka_ros/runs/5246382311?check_suite_focus=true.

[rickstaa]

@Golth, @marcbone Again CI fail doesn't give any information on how I should change the syntax.

[rickstaa]

@gollth, @Maverobot,

I've been working on resolving the CI errors we've encountered, but I could use some assistance in identifying what's going wrong. Here's what I've tried so far:

1. Executing CI Commands Locally: I attempted to run the commands specified in the .github/workflows/ci.yml file on my local machine. Surprisingly, all tests were executed without any problems, yielding only warnings and no errors.

2. Running clang-tidy-7 Locally on a File Basis: I used the command clang-tidy-7 -checks=* -header-filter=.* <FILE_NAME> to analyze the changed code locally, but I reencountered no errors.

3. Using the vscode-cpp Plugin: I also tried the clang-tidy feature in the vscode-cpp extension, but once more, no errors were detected.

For context, I've configured my settings.json as follows:

"C_Cpp.codeAnalysis.clangTidy.headerFilter": "/opt/ros/noetic/include/*:../devel/include/*",
"C_Cpp.codeAnalysis.clangTidy.path": "/usr/bin/clang-tidy-7",

Since the CI tests appear to fail despite these local checks showing no issues, I'd appreciate your insights and suggestions on pinpointing the problem. Your assistance would be invaluable in resolving this matter.

[gollth]

@rickstaa seems like the formatting is off, try running:

catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
catkin build --no-deps --make-args format -- franka_gazebo

and see if that fixes the error by running:

catkin build --no-deps --make-args check-format -- franka_gazebo

[rickstaa]

catkin build --no-deps --make-args check-format -- franka_gazebo

Ah, thanks! That makes sense. I, however, now get the following error message:

Errors     << catkin_tools_prebuild:make /home/ricks/development/work/franka_ros_ws/logs/catkin_tools_prebuild/build.make.004.log
make: *** No rule to make target 'format'.  Stop.
cd /home/ricks/development/work/franka_ros_ws/build/catkin_tools_prebuild; catkin build --get-env catkin_tools_prebuild | catkin env -si  /usr/bin/make format --jobserver-auth=3,4; cd -

It looks like the cmake folder is not found?

Ah, I found out that something went wrong during building. It was fixed after I reset/cleaned the catkin workspace 👍🏻.

[rickstaa]

Hey @gollth, it looks like the tests are now passing 😄.

Adding a simple CONTRIBUTING.md file to provide clear guidance for contributors is helpful. In my recent experience, I had to dive into the .github/workflows/ci.yml file to grasp all the necessary steps for contributing to your project. Having a CONTRIBUTING.md file would significantly enhance the user-friendliness of your repository and save contributors time when trying to contribute effectively.

This file doesn't need to be overly detailed; just a few key points could suffice:

• Instructions on how you prefer people to create pull requests, such as whether they should open an issue first or follow specific protocols.
• Recommendations for essential tools or dependencies that contributors should install when developing the project.
  Please guide the flags or commands contributors should use to ensure their pull requests align with your CI requirements.

Such a contribution guide could streamline the onboarding process for new contributors.

[rickstaa]

@gollth I've recently observed an issue stemming from the changes introduced in commit 89d2571. Specifically, after invoking either the original Gazebo set_model_configuration or the set_franka_model_configuration, the robot reverts to its initial position. This behaviour seems to be a direct consequence of the code implemented here:

franka_ros/franka_gazebo/src/franka_hw_sim.cpp

Line 496 in 5f90395

effort = positionControl(*joint, joint->stop_position, period);

To address this, I've made some adjustments in commit 6608e14, rectifying this issue and simplifying the code and the related service message.

Considering that the current Gazebo service is incompatible with your package, it might be the right moment to consider merging this pull request. This update could significantly improve the functionality and reliability of the system 🤔.

I am looking forward to your thoughts on this.

[gollth]

Hi @rickstaa, I'm not working for Franka Emika anymore. Maybe @FE-EnricoSartori can take this over?

[rickstaa]

Hi @rickstaa, I'm not working for Franka Emika anymore. Maybe @FE-EnricoSartori can take this over?

Hello @gollth, congratulations on your new position 🚀! It's great that you've tagged the appropriate person for this review—just a tiny reminder: your GitHub profile still indicates that you are employed at Franka. You might want to update that information. Best of luck in your new role!

[BarisYazici]

@rickstaa I can support you from Franka side. We are mostly focused on the ROS 2 support for FR3 robot, that's why ROS1 was left unsupported. Do you need me to review and merge the PR?

[rickstaa]

catkin build --no-deps --make-args format -- franka_gazebo

@BarisYazici, I appreciate your prompt response and the assistance you provided in getting this pull request processed. Given that ROS Noetic will reach its end-of-life in May 2025, I fully understand your team's focus on ROS2 development 👍🏻. However, I believe that approving this pull request will be advantageous. It addresses an issue where your package is incompatible with the Gazebo set_model_configuration service, as discussed in this issue. This fix is already implemented in my own franka_ros fork, which I use in my projects (ros-gazebo-gym and panda-gazebo). Integrating this change into the main repository would be beneficial for the broader community. Please feel free to request any changes you think are necessary.

[BarisYazici]

Thank you for your contribution!! 🎆 👍

[rickstaa]

Thank you for your contribution!! 🎆 👍

Hey @BarisYazici, thanks for your patience. I've just revisited the pull request and noticed that the rebase and force push inadvertently removed essential code from #226 related to the set_franka_model_configuration service. I've opened #380 to address this. Could you please review and merge it into the develop branch? My apologies for any hassle, and thanks again.