Skip to content
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

Merged
merged 20 commits into from
Aug 1, 2024

Conversation

alejomancinelli
Copy link
Contributor

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

Copy link
Contributor

@sea-bass sea-bass left a 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

  1. Make sure you don't commit the .vscode folder that your IDE may create
  2. Can you instead rebase this and put up the PR to the main branch instead of humble? That's where development happens.
  3. 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.

@alejomancinelli
Copy link
Contributor Author

Very cool to see! A few initial suggestions

1. Make sure you don't commit the .vscode folder that your IDE may create

2. Can you instead rebase this and put up the PR to the `main` branch instead of humble? That's where development happens.

3. 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.

Sure, I will do those changes. Thanks for the reply!

@sea-bass sea-bass changed the base branch from humble to main July 2, 2024 17:47
Copy link
Contributor

@sea-bass sea-bass left a 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:

  1. 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!

  2. 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

"capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
}

The sequence script creates 2 targets poses that will be reach sequentially.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
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.

Comment on lines 246 to 251
// 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
Copy link
Contributor

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:

https://github.com/ros2/examples/blob/rolling/rclcpp/actions/minimal_action_client/not_composable.cpp#L44

@alejomancinelli
Copy link
Contributor Author

Gave a more thorough look and tested it -- looks great!

Two major comments:

1. 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](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Service-And-Client.html#write-the-client-node) and [this example for actions](https://github.com/ros2/examples/blob/rolling/rclcpp/actions/minimal_action_client/not_composable.cpp#L44) -- I suspect this is why the execution fails, I even got a segfault!

2. 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

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?

@sea-bass
Copy link
Contributor

sea-bass commented Jul 3, 2024

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 pilz_move_group executable on my Rolling / Ubuntu 24.04 setup, and the example works... so I've ruled that out. Try fixing up those waits for the servers' futures?

@alejomancinelli
Copy link
Contributor Author

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.

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?

These are some of the issues I have when compiling the package

user@Lenovo:~/ROS2/moveit_fork_ws$ colcon build --packages-select moveit2_tutorials
Starting >>> moveit2_tutorials
[Processing: moveit2_tutorials]                             
--- stderr: moveit2_tutorials                               
moveit2_tutorials: You did not request a specific build type: Choosing 'Release' for maximum performance
/home/user/ROS2/moveit_fork_ws/src/moveit2_tutorials/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp:49:10: fatal error: moveit_servo/utils/common.hpp: No such file or directory
   49 | #include <moveit_servo/utils/common.hpp>
      |          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
gmake[2]: *** [doc/examples/realtime_servo/CMakeFiles/pose_tracking_tutorial.dir/build.make:76: doc/examples/realtime_servo/CMakeFiles/pose_tracking_tutorial.dir/src/pose_tracking_tutorial.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:736: doc/examples/realtime_servo/CMakeFiles/pose_tracking_tutorial.dir/all] Error 2
gmake[1]: *** Waiting for unfinished jobs....
/home/user/ROS2/moveit_fork_ws/src/moveit2_tutorials/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp: In function ‘int main(int, char**)’:
/home/user/ROS2/moveit_fork_ws/src/moveit2_tutorials/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp:156:46: error: ‘struct moveit::planning_interface::MoveGroupInterface::Plan’ has no member named ‘trajectory’; did you mean ‘trajectory_’?
  156 |   visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group);
      |                                              ^~~~~~~~~~
      |                                              trajectory_
/home/user/ROS2/moveit_fork_ws/src/moveit2_tutorials/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp:209:46: error: ‘struct moveit::planning_interface::MoveGroupInterface::Plan’ has no member named ‘trajectory’; did you mean ‘trajectory_’?
  209 |   visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group);
      |                                              ^~~~~~~~~~
      |                                              trajectory_
/home/user/ROS2/moveit_fork_ws/src/moveit2_tutorials/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp:278:46: error: ‘struct moveit::planning_interface::MoveGroupInterface::Plan’ has no member named ‘trajectory’; did you mean ‘trajectory_’?
  278 |   visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group);
      |                                              ^~~~~~~~~~
      |                                              trajectory_
/home/user/ROS2/moveit_fork_ws/src/moveit2_tutorials/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp:354:46: error: ‘struct moveit::planning_interface::MoveGroupInterface::Plan’ has no member named ‘trajectory’; did you mean ‘trajectory_’?
  354 |   visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group);
      |                                              ^~~~~~~~~~
      |                                              trajectory_
/home/user/ROS2/moveit_fork_ws/src/moveit2_tutorials/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp:406:46: error: ‘struct moveit::planning_interface::MoveGroupInterface::Plan’ has no member named ‘trajectory’; did you mean ‘trajectory_’?
  406 |   visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group);
      |                                              ^~~~~~~~~~
      |                                              trajectory_
/home/user/ROS2/moveit_fork_ws/src/moveit2_tutorials/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp:461:46: error: ‘struct moveit::planning_interface::MoveGroupInterface::Plan’ has no member named ‘trajectory’; did you mean ‘trajectory_’?
  461 |   visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group);
      |                                              ^~~~~~~~~~
      |                                              trajectory_
/home/user/ROS2/moveit_fork_ws/src/moveit2_tutorials/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp: In function ‘int main(int, char**)’:
/home/user/ROS2/moveit_fork_ws/src/moveit2_tutorials/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp:207:69: error: ‘struct planning_interface::MotionPlanResponse’ has no member named ‘error_code’; did you mean ‘error_code_’?
  207 |       planner_instance->getPlanningContext(planning_scene, req, res.error_code);
      |                                                                     ^~~~~~~~~~
      |                                                                     error_code_

I'm using ROS2 Humble on Ubuntu 22.04

@sea-bass
Copy link
Contributor

sea-bass commented Jul 8, 2024

That's an incompatibility between Humble and main. Are you also now using the main branch of moveit2 repo?

Also make sure you have uninstalled any other moveit packages from binaries: sudo apt-get remove ros-humble-moveit*

@alejomancinelli
Copy link
Contributor Author

No, my bad , I was still using the humble branch. Nevertheless, I couldn't install the main branch since I get some errors using the Humble distro. I've seen some people with the same issues, like the last comment in this issue or this issue.

Before I keep trying, do you know if this is possible? I'm following the installation steps.

@sea-bass
Copy link
Contributor

sea-bass commented Jul 8, 2024

I'm actually not sure if the main branch of MoveIt works on Humble anymore. You can try by ensuring you have removed all your MoveIt binaries and following the source instructions in a clean build.

@sea-bass
Copy link
Contributor

@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.

@alejomancinelli
Copy link
Contributor Author

Hello @sea-bass. Yes, sorry, I think is ready. I didn't know I had to do a re-request. I will merge with main and do it.

@alejomancinelli alejomancinelli requested a review from sea-bass July 31, 2024 14:56
Copy link
Contributor

@sea-bass sea-bass left a 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.

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
Copy link
Contributor

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?

Copy link
Contributor Author

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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
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.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
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.
Copy link
Contributor

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?

Copy link
Contributor Author

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." ?

Comment on lines 96 to 110
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;
Copy link
Contributor

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;
Copy link
Contributor

@sea-bass sea-bass Aug 1, 2024

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)

Copy link
Contributor Author

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

Comment on lines 276 to 281
// 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());
};
Copy link
Contributor

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?

Copy link
Contributor Author

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)

@sea-bass
Copy link
Contributor

sea-bass commented Aug 1, 2024

Update: With moveit/moveit2#2943, I could get these tutorials to run correctly on Rolling / main!

Copy link
Contributor

@stephanie-eng stephanie-eng left a 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.

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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
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".

@sea-bass
Copy link
Contributor

sea-bass commented Aug 1, 2024

@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.

I'm on Ubuntu 24.04 -- are you as well? Anyway, I found a band-aid fix in moveit/moveit2#2943

@stephanie-eng
Copy link
Contributor

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.

alejomancinelli and others added 2 commits August 1, 2024 17:56
Co-authored-by: Stephanie Eng <[email protected]>
Co-authored-by: Sebastian Castro <[email protected]>
@sea-bass sea-bass enabled auto-merge (squash) August 1, 2024 21:38
@sea-bass sea-bass disabled auto-merge August 1, 2024 21:43
@sea-bass sea-bass enabled auto-merge (squash) August 1, 2024 21:43
@sea-bass
Copy link
Contributor

sea-bass commented Aug 1, 2024

@alejomancinelli thank you again for the awesome new tutorial! 🚀

@sea-bass sea-bass merged commit 753276a into moveit:main Aug 1, 2024
8 checks passed
@gaspatxo
Copy link

gaspatxo commented Aug 2, 2024

Hello @sea-bass , I found a way of uing the MoveGroupInterface to greatly simplify the process of creating the action requests. Is that of interest? If it is, I would be happy to create a new pull request with new tutorials and everything that is needed. Here is an example, please take a look at it.

However, at least with this example I have the issue #2909, which is non-critical unless precise constant veolicty control is required

@sea-bass
Copy link
Contributor

sea-bass commented Aug 2, 2024

@gaspatxo cool! If you have time, you should put up a PR modifying this existing tutorial with whatever improvements you like.

@alejomancinelli
Copy link
Contributor Author

@alejomancinelli thank you again for the awesome new tutorial! 🚀

@sea-bass thank you and the team for all the feedback! I learn a lot with this.

@navdotnetreqs
Copy link

@gaspatxo interested in the simplified process, found this PR, did you have a change to make this other example?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
5 participants