ROS2 Gem: New components for simulating manipulators

[danielemorra98]

This PR follows the discussion about manipulation. The purpose is to develop a controller for manipulators and to build a bridge between the simulation and MoveIt.

It adds three new components:

• URDFMetadataComponent: created during the creation of a new Prefab from a URDF model, it stores the joints names and the corresponding EntityIds into a map
• JointPublishedComponent: it publishes the joints positions into /joint_states topic every simulation clock
• ManipulatorControllerComponent: it controls the hinge joints of manipulator instantiating a ROS2 Action Server (of FollowJointTrajectory type). The joints are controlled through the SetVelocity method and the commandVelocity can be computed in two ways (selectable from the Editor view):

1. Feed-Forward
2. PID

These components are supposed to make the bridge between the simulation and MoveIt.
I am opened to every kind of inputs/suggestions. I think the most critical parts are:

• The controller implementation and specifically:
• • the Action server (the tutorial implementation spawns off a thread which should execute the trajectory but since the execution should be done in the onTick function I did not find other way than using a simple State Machine regarding the status of the trajectory being executed)
• • the ExecutionTrajectory logic
• the variable type to store in URDFMetadataComponent and its consequent logic of retrieving the hingeComponent to set the velocity or get the position

Tested on:

• OS: Linux
• Version: Focal - 20.04
• CPU: 11th Gen Intel Core i9-11900K
• GPU: NVIDIA RTX A5000
• Memory: 64 Gb

URDF imported: https://github.com/danielemorra98/custom_panda_moveit/blob/master/urdf/panda.urdf
ROS2 MoveIt2 launch file: https://github.com/danielemorra98/custom_panda_moveit/blob/master/launch/panda.launch.py

[adamdbrw]

@danielemorra98 as a quick note before I review, I noticed you made some changes to ROS2Frame that might be overlapping with another PR that is going to be merged soon: #119. Please also consider rebasing on recent changes in conflicting files.

[michalpelka]

Great work! Needs some refinements, but looks awesome.
How to test this contribution?

[michalpelka]

Consider AZ::Name(name.c_str(), name.size())

[danielemorra98]

I did not find this constructor type. Are you sure about this?

[adamdbrw]

@danielemorra98 would you like to discuss the design, following the PR with design docs for manipulation? I see that you made some choices which differ from what we came up during our design discussions in the PR, I would like to understand the reasoning behind it better.

[adamdbrw]

Extract into specific control method classes

[adamdbrw]

Extract into specific control method classes

[adamdbrw]

Perhaps the best approach would be to have a RFC for the feature since it is new and quite large?

[danielemorra98]

Great work! Needs some refinements, but looks awesome. How to test this contribution?

@michalpelka you can test it importing this URDF and using this launch file. You would need to install MoveIt2 before

[adamdbrw]

@danielemorra98 thank you for addressing the high-level design changes, this looks much better now! I focused more on some details now, I believe some are quite important, please take a look at my comments.

[danielemorra98]

@adamdbrw I addressed the modifications from your latest comments, tell me what you think. Especially regarding the new map stored in the JointPublisherComponent

[adamdbrw]

@danielemorra98 thank you for applying the comments, great improvements here!
Let me know if there are unresolved comments (I resolved some but if you comment / react to each it could help).
After making sure the comments are resolved we will try the PR and we can go ahead and approve it!

[danielemorra98]

@adamdbrw Now it looks like every comment is marked as resolved

[michalpelka]

I cannot rebase the code, I cannot test it against the current version of o3de.
Could you please remove the already merged code in your contribution, and ideally rebase it on top of the current development?

[danielemorra98]

@michalpelka @adamdbrw I am facing compilation errors when I build the Project with the latest commit of both o3de and o3de-extras. Do you know if there is a stable version to build against?

[michalpelka]

@danielemorra98 to merge it need to be compiled against the development of both o3de-extras and o3de.

[danielemorra98]

@michalpelka I meant something outside from this PR:

o3de-extras/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h:71:16: error: too few template arguments for class template 'multimap'
        AZStd::multimap<AZStd::string, AZStd::string> m_status;

[michalpelka]

@danielemorra98 , I was able to compile your PR with small adjusments:

• I used o3de ("3f0b4dee328f4f5f2381b30f690132ec3c345fc4"), with the newer commits("d4488e895eb43a73d169e9faf11b8e7b7160b0b6") I encountered the same issue you mentioned.
  I had to apply fix : https://gist.github.com/michalpelka/b386e956f6084db89b6b635d2059cb43 .
  I will raise an issue, and prepare proper PR for that, so please apply this fix locally only.
• I needed to add to your code that enables exceptions in target ROS2.Static ( https://gist.github.com/michalpelka/2250cfa852968886bd081ffd58fd0ec7)
• It is great, that you managed to remove most of already merge commits from your PR, please remove (848efbd), that would intentionally revert changes from main.
  Great work within @adamdbrw on solving the issue with leaking exception requirements!

[adamdbrw]

@danielemorra98, @michalpelka Lets fix the conflicts and the header issue (which is outside of this PR), rerun the AR, confirm with a test and have this PR merged (if everyone agrees).

[lgleim]

• I used o3de ("3f0b4dee328f4f5f2381b30f690132ec3c345fc4"), with the newer commits("d4488e895eb43a73d169e9faf11b8e7b7160b0b6") I encountered the same issue you mentioned.
  I had to apply fix : https://gist.github.com/michalpelka/b386e956f6084db89b6b635d2059cb43 .
  I will raise an issue, and prepare proper PR for that, so please apply this fix locally only.

Looks like this has been addressed with #218

• I needed to add to your code that enables exceptions in target ROS2.Static ( https://gist.github.com/michalpelka/2250cfa852968886bd081ffd58fd0ec7)
• It is great, that you managed to remove most of already merge commits from your PR, please remove (848efbd), that would intentionally revert changes from main.

@danielemorra98 Can you please have a look at implementing the two remaining proposed changes?

[danielemorra98]

• I needed to add to your code that enables exceptions in target ROS2.Static ( https://gist.github.com/michalpelka/2250cfa852968886bd081ffd58fd0ec7)
• It is great, that you managed to remove most of already merge commits from your PR, please remove (848efbd), that would intentionally revert changes from main.

@michalpelka :

• I followed the suggestion coming from this comment (see here); can you confirm me that it is still not handling rclcpp exeptions (without adding the -fexceptions to the target ROS2.Static)?
• the last commit removes any modifications to the LidarFILES so that not to have any conflicts with development branch

[lgleim]

CI build passed. @michalpelka Are you still facing compilation issues regarding exception handling?

[adamdbrw]

@lgleim the CI build unfortunately does not check much as the Gem only builds with a Project that has it enabled, this is currently not checked. We can help and check this on our side with an empty project. We can check the UX as well since we are coming closer to a release (however, this PR targets development, is it intended for stabilization/release as well?)

I see there is a couple of commits which do not belong to the new changeset: 14bcd07 and 848efbd. Could we remove them from this PR? @michalpelka could assist with that.

[lgleim]

the CI build unfortunately does not check much as the Gem only builds with a Project that has it enabled, this is currently not checked.

Is anyone currently working on end-to-end testing, e.g. via python scripts?

We can help and check this on our side with an empty project. We can check the UX as well since we are coming closer to a release (however, this PR targets development, is it intended for stabilization/release as well?)

That would be much appreciated. We were targeting development based on the discussion we previously had around further refactoring the controller architecture (cf. o3de/sig-simulation#54) but would be happy to see this feature making stabilization/release

I see there is a couple of commits which do not belong to the new changeset: 14bcd07 and 848efbd. Could we remove them from this PR? @michalpelka could assist with that.

Absolutely, any help to move this forward is much appreciated.

[danielemorra98]

I see there is a couple of commits which do not belong to the new changeset: 14bcd07 and 848efbd. Could we remove them from this PR? @michalpelka could assist with that.

@adamdbrw some more recent commits undo the modification of the above mentioned ones. I probably added confusion, since I did not know how to remove those commits to the PR.

[adamdbrw]

@danielemorra98 I think it would be the best and the easiest to run an interactive rebase (git rebase -i) reaching to the very first commit in this branch. Since you are the only author of this PR and the commits would be squashed on merge anyway, you could rebase it into a single commit (squashing all but one) and then rebase that commit on the newest development branch to produce a nice and clean change-set. This would get rid of the confusing extra commits. What do you think?

[danielemorra98]

@adamdbrw now it should be ok

[michalpelka]

I was able to test PR and works as intended.
It is unstable (crashes for some user's input), but it can be merged into development and stabilized.

[lgleim]

Rerunning CI @ https://jenkins.build.o3de.org/blue/organizations/jenkins/o3de-extras/detail/PR-161/8/pipeline/

[byrcolin]

I dont see anything wrong here.

[lgleim]

Awesome, thanks again to everyone contributing to this feature and getting it up to O3DE community standards!