ros2 branch was cleaned up and force-pushed

[rhaschke]

I went through all commits pushed onto the ros2 branch in recent months from PickNik's side and cleaned them up to the standards of this repo.
Many of them I moved to the master branch and eventually merged them back into the ros2 branch.
Using merging instead of cherry-picking, one can more easily see which commits are on which branch.

Attention: I force-pushed the ros2 branch to the new, cleaned version.
Please update your local workspaces, resetting your local ros2 branch: git remote update; git reset --hard origin/ros2.

Only a few commits, whose value I still consider questionable, are left on the ros2 branch. These include:

• Enable parallel planning with PipelinePlanner #450 (I think a dedicated solver is more suitable in the context of MTC)
• Add planner name to trajectory info #490 (this info is not displayed anywhere, integrating it into the comment makes more sense - which was realized in Add planner info to MTC solution comment.  #523)

Some others are ros2-specific, for example:

• Run goalcallback asynchronously #496 (only relevant for the ros2 action server)
• Update to the more recent JumpThreshold API #506

For now, I kept the old ros2 branch with name ros2-old. I will delete this branch in a few weeks.
The new branch should™ include all functionality of the old one as well as all improvements from the master branch.
See here for all differences. To highlight my changes even more, I created a cleanup branch, where individual changes can be seen more easily.
In the new ros2 branch, those cleanups are squash-merged into the corresponding main commits, though.

Comparing PickNik's ros2 branch with the old ros2 branch, apart from many missing commits, I noticed larger deviations in ExecuteTaskSolutionCapability. So far, I don't understand, why you, @henningkayser, refactored this code so much. In any case, I think you should consider switching to the new ros2 branch.

[sea-bass]

Force pushing to a major branch and rewriting history is not ideal for repos that have a significant number of users such as this one. Especially when (not hypothetically) people may depend on specific commit hashes for their work.

IMO it would have been preferable to do a "normal" pull request with your intended improvements/cleanups and tag the authors of the disputed PRs for review so that they can learn from your feedback.

Additionally to the logistics of having to do hard resets locally, and having to re-test things on our side, it seems like a healthier way to interact with other maintainers rather than saying "your code was so bad that I'm replacing it and deleting it in 2 weeks, good luck".

[sea-bass]

Also unrelated to my personal opinion, I just ran the regular MTC tutorial on MoveIt2 with Rolling:

ros2 launch moveit2_tutorials mtc_demo.launch.py
ros2 launch moveit2_tutorials pick_place_demo.launch.py

On the ros2-old branch, this runs as expected; on the new ros2 branch, it gets stuck spamming output like this without completing a plan:

[mtc_tutorial-1] [INFO] [1710159948.866114443] [planning_scene_interface_101210164031856.moveit.ompl_model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_tutorial-1] [INFO] [1710159948.866167638] [mtc_node]: Calling Planner 'OMPL'
[mtc_tutorial-1] [INFO] [1710159948.878990052] [mtc_node]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[mtc_tutorial-1] [INFO] [1710159948.879465023] [mtc_node]: Calling PlanningResponseAdapter 'ValidateSolution'
[mtc_tutorial-1] [INFO] [1710159948.879653293] [mtc_node]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[mtc_tutorial-1] [WARN] [1710159948.879669334] [planning_scene_interface_101210164031856.moveit.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[mtc_tutorial-1] [WARN] [1710159948.985755298] [planning_scene_interface_101210164031856.moveit.cartesian_interpolator]: The computed path is too short to detect jumps in joint-space. Need at least 10 steps, only got 2. Try a lower max_step.
[mtc_tutorial-1] [WARN] [1710159949.090018639] [planning_scene_interface_101210164031856.moveit.cartesian_interpolator]: The computed path is too short to detect jumps in joint-space. Need at least 10 steps, only got 2. Try a lower max_step.
[mtc_tutorial-1] [WARN] [1710159949.193853728] [planning_scene_interface_101210164031856.moveit.cartesian_interpolator]: The computed path is too short to detect jumps in joint-space. Need at least 10 steps, only got 2. Try a lower max_step.
[mtc_tutorial-1] [WARN] [1710159949.299557707] [planning_scene_interface_101210164031856.moveit.cartesian_interpolator]: The computed path is too short to detect jumps in joint-space. Need at least 10 steps, only got 2. Try a lower max_step.
[mtc_tutorial-1] [WARN] [1710159949.438257921] [planning_scene_interface_101210164031856.moveit.cartesian_interpolator]: The computed path is too short to detect jumps in joint-space. Need at least 10 steps, only got 2. Try a lower max_step.

It's planning for a long time and unable to find plans that meet the complete goal...

[mtc_tutorial-1]   0  - ←   0 →   -  0 / demo task
[mtc_tutorial-1]     1  - ←   1 →   -  0 / current
[mtc_tutorial-1]     -  0 →   1 →   -  1 / open hand
[mtc_tutorial-1]     -  1 →   0 ←  91  - / move to pick
[mtc_tutorial-1]    91  - ←  91 →   - 91 / pick object
[mtc_tutorial-1]      92  - ←  92 ←   5  - / approach object
[mtc_tutorial-1]       5  - ← 110 →   -  5 / grasp pose IK
[mtc_tutorial-1]        25  - ←  25 →   - 25 / generate grasp pose
[mtc_tutorial-1]       -  5 → 105 →   -  0 / allow collision (hand,object)
[mtc_tutorial-1]       -  0 → 105 →   -  0 / close hand
[mtc_tutorial-1]       -  0 → 105 →   -  0 / attach object
[mtc_tutorial-1]       -  0 →  99 →   - 99 / lift object
[mtc_tutorial-1]     - 91 →   0 ← 396  - / move to place
[mtc_tutorial-1]   396  - ← 396 →   -  0 / place object
[mtc_tutorial-1]     1427  - ← 1427 →   -  0 / place pose IK
[mtc_tutorial-1]       1050  - ← 1050 →   -1050 / generate place pose
[mtc_tutorial-1]       -  0 → 1427 →   -  0 / open hand
[mtc_tutorial-1]       -  0 → 1427 →   -  0 / forbid collision (hand,object)
[mtc_tutorial-1]       -  0 → 1427 →   -  0 / detach object
[mtc_tutorial-1]       -  0 → 396 →   -396 / retreat
[mtc_tutorial-1]     -  0 → 392 →   -392 / return home
[mtc_tutorial-1] Failing stage(s):
[mtc_tutorial-1] move to pick (0/91): Trajectory end-point deviates too much from goal state

Seems related to this newly created issue: moveit/moveit2_tutorials#881

So it seems that some key functionality to get this working with MoveIt 2 main has been missed in this transition. Any ideas?

[sjahr]

I share Sebastian's opinion regarding force pushing changes on the ros2 branch. Do you mind reverting that and opening a clean-up PR to review your changes properly? Regarding your comment:

I went through all commits pushed onto the  branch in recent months from PickNik's side and cleaned them up to the standards of this repo.

I think this statement is very dismissive of a lot of peoples work. All of the contributions to the ros2 branch are from individuals who value high code quality and want to improve MTC and not "from PickNik". None of these changes were pushed onto the ros2 branch from PickNik but a PR was opened and the changes underwent a proper review process. I think all of the PRs with major changes have been open for at least one 1 week for other maintainers to review.

[rhaschke]

Regarding the failing pick+place demo. Looks like you have more sloppy goal constraints in ROS2 by default.
Increasing the (newly introduced) max goal deviation check in Connect to 1e-2 resolves the planning issue for me:
https://github.com/ros-planning/moveit_task_constructor/blob/0c4b4fcaa89992f8b0a6f19a46342bd51352c5c9/core/src/stages/connect.cpp#L60

[sea-bass]

Oh, awesome, thank you! Will take a look at that.

[AndyZe]

I'll just comment (from vacation) that I certainly see both sides of this issue. There have been some indications recently that MoveIt PRs are not getting reviewed as well as they used to, and that's not surprising with all the staff turnover at PickNik. For example, I saw on Robotics Stack Exchange that MSA is broken (apparently). Suggest a 2-week waiting period before merging MRs. 1 week sometimes isn't enough for people like myself (not with PickNik) to review.

…

On Mon, Mar 11, 2024, 6:49 AM Sebastian Jahr ***@***.***> wrote: I share Sebastian's opinion regarding force pushing changes on the ros2 branch. Do you mind reverting that and opening a clean-up PR to review your changes properly? Regarding your comment: I went through all commits pushed onto the branch in recent months from PickNik's side and cleaned them up to the standards of this repo. I think this statement is very dismissive of a lot of peoples work. All of the contributions to the ros2 branch are from individuals who value high code quality and want to improve MTC and not from "PickNik". None of these changes were pushed onto the ros2 branch from PickNik but a PR was opened and the changes underwent a proper review process. I think all of the PRs with major changes have been open for at least one 1 week for other maintainers to review. — Reply to this email directly, view it on GitHub <#540 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ACWC7KO7LBTQD6T6LEYXCI3YXWR7BAVCNFSM6AAAAABEO3G6ZKVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMYTSOBYGM3DMNZTGU> . You are receiving this because you are subscribed to this thread.Message ID: ***@***.*** com>

[henningkayser]

I think force pushing to an actively used branch is really a no-go. And this has not been the first time where active dependencies that we maintain and share ownership are being changed or removed under us. We are happy to discuss any quality issues and collaborate by following standards, but in this case it seems like they are arbitrarily self-imposed and not documented anywhere.

[rhaschke]

To clarify: my major goal was to cleanup the history of the ros2 branch, backporting some commits to the master branch and then performing a merge-commit to integrate those (and all other improvements on master) into the ros2 branch. This can only be achieved with a force-push. I explicitly kept ros2-old to facilitate migration.

Probably due to your "sloppy" review process, there were several fixup commits, which should have been ideally handled as part of the reviews. I squashed those into their main commit. I also found 1-2 newly introduced bugs, which I fixed. @sea-bass, if you want to learn from my fixup commits, check out the ros2-cleanup branch, which I explicitly maintained and linked for teaching purposes.

Regarding the standards not maintained by some commits: There were some commits not following basic formatting standards, which should be actually imposed by CI. I'm not sure how these slipped through, but it happened - multiple times! This was also not noticed during the review process!

At some point, we had a policy for MoveIt1 that we need reviews from two people from independent organizations. This rule was clearly violated for most of the commits (if not all), because the PR, reviews, and the final merge were all performed by PickNik people. I still see the main responsibility for this repository with Michael and me. In regard of this responsibility I want to avoid a major divergence between the master and ros2 branches.

[rhaschke]

Regarding functionality: With the max_distance parameter of the Connect stage increased (see #540 (comment)), I still face issues executing the planned trajectory:

[ros2_control_node-5] [ERROR] [1710170805.182119610] [panda_arm_controller]: Velocity of last trajectory point of joint panda_joint1 is not zero: -0.000523622440279
[move_group-4] [INFO] [1710170805.182243269] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: panda_arm_controller started execution
[move_group-4] [WARN] [1710170805.182252011] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request rejected
[move_group-4] [ERROR] [1710170805.182278182] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal was rejected by server
[move_group-4] [ERROR] [1710170805.182299582] [move_group.moveit.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller panda_arm_controller
[move_group-4] [INFO] [1710170805.182313773] [move_group.moveit.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...

As I face similar issues with the ros2-old branch, I think this is an issue in MoveIt 2.

Edit: This issue is a duplicate of moveit/moveit2_tutorials#881, which is resolved by moveit/moveit_resources#198.

[sjahr]

At some point, we had a policy for MoveIt1 that we need reviews from two people from independent organizations.

I couldn't find that in moveit's maintainer policy. Where does this come from?

Also I am wondering what this statement implies:

I still see the main responsibility for this repository with Michael and me. In regard of this responsibility I want to avoid a major divergence between the master and ros2 branches.

Does moveit's maintainer policy in your opinion fully apply to this repository and all maintainers are considered equal?

In general I think we should discuss these issues regarding the review process and the maintainer policy in the next maintainer meeting to ensure we have a healthy and sustainable way to cooperate. @rhaschke are you available to join?

[rhaschke]

When is the next maintainer meeting scheduled?

[sjahr]

March 28, you find the meeting link in the discourse announcement https://discourse.ros.org/t/manipulation-wg-moveit-maintainer-meeting-march-28/36404.

[yangming517]

Also unrelated to my personal opinion, I just ran the regular MTC tutorial on MoveIt2 with Rolling:也与我的个人意见无关，我只是在 MoveIt2 上使用 Rolling 运行了常规的 MTC 教程：

ros2 launch moveit2_tutorials mtc_demo.launch.py
ros2 launch moveit2_tutorials pick_place_demo.launch.py

On the ros2-old branch, this runs as expected; on the new ros2 branch, it gets stuck spamming output like this without completing a plan:在分支上 ros2-old ，这将按预期运行;在新 ros2 分支上，它会在没有完成计划的情况下卡住这样的垃圾邮件输出：

[mtc_tutorial-1] [INFO] [1710159948.866114443] [planning_scene_interface_101210164031856.moveit.ompl_model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_tutorial-1] [INFO] [1710159948.866167638] [mtc_node]: Calling Planner 'OMPL'
[mtc_tutorial-1] [INFO] [1710159948.878990052] [mtc_node]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[mtc_tutorial-1] [INFO] [1710159948.879465023] [mtc_node]: Calling PlanningResponseAdapter 'ValidateSolution'
[mtc_tutorial-1] [INFO] [1710159948.879653293] [mtc_node]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[mtc_tutorial-1] [WARN] [1710159948.879669334] [planning_scene_interface_101210164031856.moveit.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[mtc_tutorial-1] [WARN] [1710159948.985755298] [planning_scene_interface_101210164031856.moveit.cartesian_interpolator]: The computed path is too short to detect jumps in joint-space. Need at least 10 steps, only got 2. Try a lower max_step.
[mtc_tutorial-1] [WARN] [1710159949.090018639] [planning_scene_interface_101210164031856.moveit.cartesian_interpolator]: The computed path is too short to detect jumps in joint-space. Need at least 10 steps, only got 2. Try a lower max_step.
[mtc_tutorial-1] [WARN] [1710159949.193853728] [planning_scene_interface_101210164031856.moveit.cartesian_interpolator]: The computed path is too short to detect jumps in joint-space. Need at least 10 steps, only got 2. Try a lower max_step.
[mtc_tutorial-1] [WARN] [1710159949.299557707] [planning_scene_interface_101210164031856.moveit.cartesian_interpolator]: The computed path is too short to detect jumps in joint-space. Need at least 10 steps, only got 2. Try a lower max_step.
[mtc_tutorial-1] [WARN] [1710159949.438257921] [planning_scene_interface_101210164031856.moveit.cartesian_interpolator]: The computed path is too short to detect jumps in joint-space. Need at least 10 steps, only got 2. Try a lower max_step.

It's planning for a long time and unable to find plans that meet the complete goal...它计划了很长时间，无法找到满足完整目标的计划......

[mtc_tutorial-1]   0  - ←   0 →   -  0 / demo task
[mtc_tutorial-1]     1  - ←   1 →   -  0 / current
[mtc_tutorial-1]     -  0 →   1 →   -  1 / open hand
[mtc_tutorial-1]     -  1 →   0 ←  91  - / move to pick
[mtc_tutorial-1]    91  - ←  91 →   - 91 / pick object
[mtc_tutorial-1]      92  - ←  92 ←   5  - / approach object
[mtc_tutorial-1]       5  - ← 110 →   -  5 / grasp pose IK
[mtc_tutorial-1]        25  - ←  25 →   - 25 / generate grasp pose
[mtc_tutorial-1]       -  5 → 105 →   -  0 / allow collision (hand,object)
[mtc_tutorial-1]       -  0 → 105 →   -  0 / close hand
[mtc_tutorial-1]       -  0 → 105 →   -  0 / attach object
[mtc_tutorial-1]       -  0 →  99 →   - 99 / lift object
[mtc_tutorial-1]     - 91 →   0 ← 396  - / move to place
[mtc_tutorial-1]   396  - ← 396 →   -  0 / place object
[mtc_tutorial-1]     1427  - ← 1427 →   -  0 / place pose IK
[mtc_tutorial-1]       1050  - ← 1050 →   -1050 / generate place pose
[mtc_tutorial-1]       -  0 → 1427 →   -  0 / open hand
[mtc_tutorial-1]       -  0 → 1427 →   -  0 / forbid collision (hand,object)
[mtc_tutorial-1]       -  0 → 1427 →   -  0 / detach object
[mtc_tutorial-1]       -  0 → 396 →   -396 / retreat
[mtc_tutorial-1]     -  0 → 392 →   -392 / return home
[mtc_tutorial-1] Failing stage(s):
[mtc_tutorial-1] move to pick (0/91): Trajectory end-point deviates too much from goal state

Seems related to this newly created issue: ros-planning/moveit2_tutorials#881似乎与这个新创建的问题有关：moveit/moveit2_tutorials#881

So it seems that some key functionality to get this working with MoveIt 2 main has been missed in this transition. Any ideas?因此，在此过渡中，似乎缺少了一些与 MoveIt 2 main 配合使用的关键功能。有什么想法吗？

Hi,friend,Is there a solution to this problem now. I am currently troubled by this issue.Hope your reply,thank you~
ros2 (humble)
moveit2(source build -humble)
moveit2_tutorials(branch -humble)

[rhaschke]

@yangming517: check #542 and moveit/moveit_resources#198 for a workaround for these issues

[yangming517]

I have update my local workspaces, resetting my local ros2 branch: git remote update; git reset --hard origin/ros2.
However,it don't work. @rhaschke

[rhaschke]

Please describe more precisely, what you did, @yangming517. I suggested to use both of the mentioned PRs. Did you do that? What exactly is the remaining error message? The statement "It doesn't work" is not very helpful in determining a cause.

[yangming517]

@rhaschke sorry for my vague explanation.
checking #542 , I have done :git remote update; git reset --hard origin/ros2 in "moveit_task_constructor" folder
checking #198 , add allow_nonzero_velocity_at_trajectory_end: true to the ros_controllers.yaml of moveit_resources_panda_moveit_config .
at last,colcon build --mixin release .colcon build successful .
but the terminal still shows above,which is same as [sea-bass]
`[mtc_tutorial-1] [WARN] [1710751114.988806295] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 3. Try a lower max_step.
[mtc_tutorial-1] 0 - ← 0 → - 0 / demo task
[mtc_tutorial-1] 1 - ← 1 → - 0 / current
[mtc_tutorial-1] - 0 → 1 → - 1 / open hand
[mtc_tutorial-1] - 1 → 0 ← 106 - / move to pick
[mtc_tutorial-1] 106 - ← 106 → -106 / pick object
[mtc_tutorial-1] 108 - ← 108 ← 6 - / approach object
[mtc_tutorial-1] 6 - ← 130 → - 6 / grasp pose IK
[mtc_tutorial-1] 25 - ← 25 → - 25 / generate grasp pose
[mtc_tutorial-1] - 6 → 124 → - 0 / allow collision (hand,object)
[mtc_tutorial-1] - 0 → 124 → - 0 / close hand
[mtc_tutorial-1] - 0 → 124 → - 0 / attach object
[mtc_tutorial-1] - 0 → 116 → -116 / lift object
[mtc_tutorial-1] -106 → 0 ← 487 - / move to place
[mtc_tutorial-1] 487 - ← 487 → - 0 / place object
[mtc_tutorial-1] 1703 - ← 1703 → - 0 / place pose IK
[mtc_tutorial-1] 1240 - ← 1240 → -1240 / generate place pose
[mtc_tutorial-1] - 0 → 1703 → - 0 / open hand
[mtc_tutorial-1] - 0 → 1703 → - 0 / forbid collision (hand,object)
[mtc_tutorial-1] - 0 → 1703 → - 0 / detach object
[mtc_tutorial-1] - 0 → 487 → -487 / retreat
[mtc_tutorial-1] - 0 → 479 → -479 / return home
[mtc_tutorial-1] Failing stage(s):
[mtc_tutorial-1] move to pick (0/106): Trajectory end-point deviates too much from goal state
[mtc_tutorial-1] [ERROR] [1710751115.207957029] [mtc_tutorial]: Task planning failed

Is there anything I missed? If you could point it out, I would greatly appreciate it.
`

[rhaschke]

@yangming517, you didn't yet pull #542. In your moveit_task_constructor folder do the following:

git fetch https://github.com/ubi-agni/moveit_task_constructor ros2
git checkout FETCH_HEAD

After this, your commit SHA should be b40e932.

[yangming517]

@rhaschke hello Regarding my question, it seems that the key should be the modification of this codep.declare<double>("max_distance", 1e-4, "maximally accepted distance between end and goal sate"); Finally problem was resolved and the demo ran successfully. Thank you for your help.

[rhaschke]

When is the next maintainer meeting scheduled?

I cannot join on March 28th, unfortunately.
Regarding the merge/commit policy: I think that merge commits should be limited primarily to Michael and me for this repo. I really try to be responsive, particularly on this repo. If I'm not, don't hesitate to ping me.
In general, I like to keep the master and ros2 branches in sync as far as possible, ideally only having ros2-required changes in ros2 (but no functional deviations). Merging some PRs w/o approval from Michael or me, other MoveIt maintainers deviated from this policy in the past, which is why I cleaned up history. Although I still don't like your approach, @sjahr, to parallel planning pipelines (it is not MTC-conform), I kept the corresponding commit(s) as you seem to rely on that feature.