-
Notifications
You must be signed in to change notification settings - Fork 196
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
Pilz Sequence Motion MoveGroupSequence example #917
Conversation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Very cool to see! A few initial suggestions
- Make sure you don't commit the .vscode folder that your IDE may create
- Can you instead rebase this and put up the PR to the
main
branch instead of humble? That's where development happens. - Also, make sure you modify the actual docs page to add instructions for your great new example!
Try making those changes and I'll do a more thorough review/test after.
doc/examples/pilz_industrial_motion_planner/src/pilz_sequence.cpp
Outdated
Show resolved
Hide resolved
Sure, I will do those changes. Thanks for the reply! |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Gave a more thorough look and tested it -- looks great!
Two major comments:
-
I think calling and waiting for the service and action results may be done incorrectly, as
future.get()
does not block. You should do something like what is shown in this ROS 2 tutorial and this example for actions -- I suspect this is why the execution fails, I even got a segfault! -
The docs show the action interface and then the service interface first, but the example does service and then action. I recommend shuffling the docs to also do first service and then action.
BTW were you getting these errors?
[ros2_control_node-5] [ERROR] [1719959728.962806900] [panda_arm_controller]:
Time between points 27 and 28 is not strictly increasing, it is 2.700000 and 2.700000 respectively
doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst
Outdated
Show resolved
Hide resolved
doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py
Outdated
Show resolved
Hide resolved
"capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" | ||
} | ||
|
||
The sequence script creates 2 targets poses that will be reach sequentially. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The sequence script creates 2 targets poses that will be reach sequentially. | |
The `pilz_sequence.cpp file` creates 2 target poses that will be reached sequentially. |
... and see if you can directly link to the source code with the :codedir:
directive.
doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst
Outdated
Show resolved
Hide resolved
doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst
Outdated
Show resolved
Hide resolved
doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py
Show resolved
Hide resolved
// Get result | ||
auto future_result = client->async_get_result(goal_handle_future.get()); | ||
|
||
// Wait for the result | ||
if (future_result.valid()) { | ||
auto result = future_result.get(); // Blocks the program execution until it gets a response |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
same here, this needs to wait for the action to complete. You can use the same "loop and check if complete" paradigm from the service example, or just spin this node until the action is complete:
doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp
Outdated
Show resolved
Hide resolved
doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp
Outdated
Show resolved
Hide resolved
Co-authored-by: Sebastian Castro <[email protected]>
Nop, I didn´t get any errors. Did you changed or added points in the trajectory? I could try to replicate it. For (2) I think you are right, I already change the order in the tutorial. One last thing: after doing the main rebase, I'm getting error in others examples that I didn´t modify, so I can´t compile the package. Do you know how to solve this? |
It's possible that my trajectory errors show up because I am testing on rolling, not Humble. Similarly, versions of packages could be causing compilation errors on your end. What are you seeing? EDIT: BTW I also tried running the regular |
I have changed the server's future. Now it waits with a while loop and future.wait_for() unitil it is ready. I still have to update the tutorial, let me know if the changes are ok to do so.
These are some of the issues I have when compiling the package
I'm using ROS2 Humble on Ubuntu 22.04 |
That's an incompatibility between Humble and main. Are you also now using the Also make sure you have uninstalled any other moveit packages from binaries: |
No, my bad , I was still using the Before I keep trying, do you know if this is possible? I'm following the installation steps. |
I'm actually not sure if the |
@alejomancinelli is this ready for review again, or are you still working on it? If it is ready, re-request and I will test it out again. |
Hello @sea-bass. Yes, sorry, I think is ready. I didn't know I had to do a re-request. I will merge with |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thank you for the great work!
So, everything seems to work on your end, given the screenshots, but on rolling / main I get an error with the controller:
[ros2_control_node-5] [ERROR] [1722470751.608592463] [panda_arm_controller]:
Time between points 27 and 28 is not strictly increasing, it is 2.700000 and 2.700000 respectively
Part of me wants to just accept this and then raise an issue on the MoveIt repo to try resolve this on the Pilz motion planner side. (EDIT: This is already an issue moveit/moveit2#2741)
So I'm tagging @sjahr, @stephanie-eng, and @henningkayser to see what they think.
But barring some very minor comments, LGTM.
doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst
Outdated
Show resolved
Hide resolved
service_request->request.items.push_back(item1); | ||
service_request->request.items.push_back(item2); | ||
|
||
Service call and response. The method ``future.wait_for(timeout_duration)`` waits for the |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This "Service call and response" is not a full sentence, could you reword?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I changed the whole sentence to "Once the service call is completed, the method future.wait_for(timeout_duration)
blocks until a specified timeout_duration
has elapsed or the result becomes available, whichever comes first. The return value identifies the state of the result. This is performed every second until the future is ready."
Service call and response. The method ``future.wait_for(timeout_duration)`` waits for the | ||
result to become available. Blocks until specified ``timeout_duration`` has elapsed or the | ||
result becomes available, whichever comes first. The return value identifies the state of | ||
the result. This is perform every 1 seconds until the future is ready. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
the result. This is perform every 1 seconds until the future is ready. | |
the result. This is performed every 1 second until the future is ready. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
the result. This is perform every 1 seconds until the future is ready. | |
the result. This is perform every second until the future is ready. |
Nitpick again, apologies! Every one second is a bit awkward sounding, in my opinion. Consider "Once a second", "Every second".
sequence_request.items.push_back(item1); | ||
sequence_request.items.push_back(item2); | ||
|
||
Create goal and planning options. A goal response callback and result callback can be included as well. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Same here: "Create goal and planning options" isn't quite a full sentence, could you reword?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Something simple like "The goal and planning options are configured." ?
msg.pose.orientation.y = 0; | ||
msg.pose.orientation.z = 0; | ||
msg.pose.orientation.w = 0; | ||
return msg; | ||
}(); | ||
item1.req.goal_constraints.push_back( | ||
kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); | ||
|
||
// ----- Motion Sequence Item 2 | ||
// Create a MotionSequenceItem | ||
moveit_msgs::msg::MotionSequenceItem item2; | ||
|
||
// Set pose blend radius | ||
// For the last pose, it must be 0! | ||
item2.blend_radius = 0; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
nit: For all these values, in this file and the docs, could you replace the 0
with 0.0
?
And same with the 5
in the planning time.
// Planning options | ||
goal_msg.planning_options.planning_scene_diff.is_diff = true; | ||
goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true; | ||
// goal_msg.planning_options.plan_only = true; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
remove or uncomment?
(or leave a comment next to this saying users can change this to do only planning)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I added a "Uncomment to only plan the trajectory" comment since I think this option could be useful for someone
// Feedback callback | ||
// It does not show the state of the sequence, but the state of the robot: PLANNING, MONITOR, IDLE | ||
send_goal_options.feedback_callback = [](GoalHandleMoveGroupSequence::SharedPtr, | ||
const std::shared_ptr<const GoalHandleMoveGroupSequence::Feedback> feedback) { | ||
RCLCPP_INFO(LOGGER, "Feedback: %s", feedback->state.c_str()); | ||
}; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Do we need a feedback callback?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Actually, no. I thought the feedbak would return the next point in the sequence during the execution, but since it only returns the state of the action is not really that usefull (at least not in this example)
Update: With moveit/moveit2#2943, I could get these tutorials to run correctly on Rolling / main! |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thank you for writing this! I have some minor wording suggestions, but the overall content is great!
@sea-bass I am unable to reproduce the issue you saw with the duplicated time points on rolling, but in my opinion, that is a MoveIt / Pilz issue and shouldn't block the addition of this tutorial.
doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst
Outdated
Show resolved
Hide resolved
doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst
Outdated
Show resolved
Hide resolved
doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst
Outdated
Show resolved
Hide resolved
doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst
Outdated
Show resolved
Hide resolved
Service call and response. The method ``future.wait_for(timeout_duration)`` waits for the | ||
result to become available. Blocks until specified ``timeout_duration`` has elapsed or the | ||
result becomes available, whichever comes first. The return value identifies the state of | ||
the result. This is perform every 1 seconds until the future is ready. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
the result. This is perform every 1 seconds until the future is ready. | |
the result. This is perform every second until the future is ready. |
Nitpick again, apologies! Every one second is a bit awkward sounding, in my opinion. Consider "Once a second", "Every second".
doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst
Outdated
Show resolved
Hide resolved
I'm on Ubuntu 24.04 -- are you as well? Anyway, I found a band-aid fix in moveit/moveit2#2943 |
I'm on 22.04 host but I run everything in the osrf/ros:rolling-desktop container which is 24.04, so should be similar. |
Co-authored-by: Stephanie Eng <[email protected]> Co-authored-by: Sebastian Castro <[email protected]>
doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst
Outdated
Show resolved
Hide resolved
doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst
Outdated
Show resolved
Hide resolved
@alejomancinelli thank you again for the awesome new tutorial! 🚀 |
Hello @sea-bass , I found a way of uing the However, at least with this example I have the issue #2909, which is non-critical unless precise constant veolicty control is required |
@gaspatxo cool! If you have time, you should put up a PR modifying this existing tutorial with whatever improvements you like. |
@sea-bass thank you and the team for all the feedback! I learn a lot with this. |
@gaspatxo interested in the simplified process, found this PR, did you have a change to make this other example? |
MoveGroupSequence example for generating a sequence of trajectories with a blend radius.
pilz_moveit.launch.py replaces mtc_demo.launch.py since it needs to include the action server and service server capabilities.
pilz_sequence.cpp plans and shows a trajectory using the MoveGroupSequence service, then plans and executes the trajectory using the MoveGroupSequence action. Finally, it executes the same sequence, but stops the motion after 2 seconds.
@sea-bass