Pilz Sequence Motion MoveGroupSequence example

alejomancinelli commented 4 months ago

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

alejomancinelli commented 4 months ago

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!

alejomancinelli commented 4 months ago

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 commented 4 months ago

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 commented 4 months ago

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 commented 4 months ago

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 commented 4 months ago

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 commented 4 months ago

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.