Hybrid Planning: Add review changes to feature branch and rebase #811
Conversation
* Add hybrid_planning architecture skeleton code * Add simple state machines to hybrid planning manager and local planner * Initial global planner prototype implementation * Forward joint_trajectory with local planner * Forward hybrid planning motion request to global planner * Abstract planner logic from hybrid planning manager by using a plugin * Implement single plan execution logic * Add test launch files, RViz and demo config * Add test for motion_planning_request
* Make local planner component generic * Add next_waypoint_sampler trajectory operator * Update hybrid planning test to include collision object * Clean up code and fix minor bugs. * Update local planner component parameter * Add local collision constraint solver * Update planning scene topic name and test node * Fix bugs and runtime errors in local planner component and it's plugins * Add collision object that invalidates global trajectory * Add simple "stop in front of collision object" demo * Add hybrid planning manager reaction to local planner feedback * Fix ament_lint_cmake * Ensure that local planner start command and collision event are send once * Add simple replanning logic plugin * Use current state as start state for global planner * Use RobotTrajectory instead of constraint vector describe local problem * Add PlanningSceneMonitorPtr to local solver plugin * Add local planner frequency parameter * Use PID controller to create control outputs for robot controller * Add hybrid_planning_manager config file * Add more complex test node * Update README * Reset index in next_waypoint_sampler * Use correct isPathValid() interface * Rename path_invalidation flag * Read planning scene instead of cloning it in local planner * Add TODO creator * Rename local constraint solver plugin * Use read-locked access to the planning scene for collision checking * Rename constraint_solver into local_constraint_solver * Add missing pointer initialization
* Export missing plugins * Use std::chrono_literals * Construct smart pointers with std::make_* instead of 'new' * Fixup const-ref in function signatures, reduce copies * Improve planning scene locking, robot state processing, controller logic
* Add forward_trajectory local solver plugin (moveit#359) * Use ros2_control based joint simulation and remove unnecessary comment * Update copyrights * Restructure hybrid planning package * Fix formatting and add missing time stamp in local solver plugin * Remove unnecessary logging and param loading * Enable different interfaces between local planner and controller * Use JointGroupPositionController as demo hardware controller
* Add reset method for trajectory operator and local constraint sampler * Refactor next_waypoint_sampler into simple_sampler * Include collision checking to forward_trajectory and remove unneeded plugin * Fix formatting and plugin description * Update README and add missing planner logic plugin description
|
|
||
| // Forward global trajectory goal from hybrid planning request TODO(sjahr) pass goal as function argument | ||
| auto global_goal_msg = moveit_msgs::action::GlobalPlanner::Goal(); | ||
| global_goal_msg.motion_sequence = |
There was a problem hiding this comment.
This causes a segfault right now
|
I appreciate you opening a new PR from a branch I have access to. I'm pretty deep into addressing the code reviews on a branch of my own. I plan to push onto this branch soon. Sound good? |
Sounds perfect thanks! I can review those and maybe add some minor things that are left |
|
@AndyZe FYI I've marked the review comments that I've addressed so far with 👍 in the old PR |
@AndyZe no worries, my only concern is that you are doing some duplicate work as I've addressed a lot of the review comments in #718/ #813 but I understand that it might take you more time to unify both branches instead of simply fixing those by yourself. I've mostly addressed #814 with a985b8c, b027f62 and 5682263 of this PR. I've removed the timers and added another member class to the hybrid planning manager which contains the ros2 communication interfaces (publisher, subscriber etc). Thereby, I can give the planning logic access to those, without the need of passing a shared pointer of the whole hybrid planning manager to the logic. The only thing that does not work right now, is the thing I've pointed out in a comment in this PR: After successfully moving it in the member variable in line 214, the goal_handle becomes invalid when next used in line 141. I think the first commit of the 3 mentioned above can be cherry-picked without concern, because the segfault I've experienced is caused in the hybrid_planning_manager after adding the latter two. |
Can we get this huge chunk of code merged into the feature branch first? It's much easier to follow what's happening if there are not plenty of different authors pushing to multiple PR branches. @sjahr if your intention is to get this merged into main (without a squash), then the commits should be squashed down a bit more. "Clean up..." "Rename..." "Refactor..." should be part of the commits that added the features. |
Description
This PR reopens #718 with the intention to get the hybrid planning feature merged into main.
I've rebased again on main (11/18/2021) and added the changes introduced by #763 (Thanks to @tylerjw and @AndyZe!). Furthermore, I've fixed some things to make the demo work again, although that is still work in progress right now.
@AndyZe and @tylerjw I've sent you collaborator invitations to my moveit2 fork, so you can directly contribute to this PR to speed up the process.
Checklist