Skip to content

WIP: Refactor hybrid planning manager#5

Closed
sjahr wants to merge 11 commits intoAndyZe:feature/hybrid_planningfrom
sjahr:pr-refactor_hybrid_planning_manager
Closed

WIP: Refactor hybrid planning manager#5
sjahr wants to merge 11 commits intoAndyZe:feature/hybrid_planningfrom
sjahr:pr-refactor_hybrid_planning_manager

Conversation

@sjahr
Copy link
Copy Markdown

@sjahr sjahr commented Nov 30, 2021

Description

I've refactored the Hybrid Planning Manager Class to avoid the ugly hacks @tylerjw pointed out. The problem I encountered is that it is not straightforward to convert the Manager into a Node Class because it shares its ROS 2 communication interfaces with the planner logic plugin. I've tried to fix this by moving the interfaces into a second class called HybridPlannerInterface and thus avoid giving a shared_pointer of the HybridPlanningManager to the PlannerLogicPlugin.

Currently, this does not work because a segfault error occurs at runtime which needs to be debugged :/

Checklist

  • Required by CI: Code is auto formatted using clang-format
  • Fix Segfault

sjahr and others added 10 commits November 29, 2021 08:34
* 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

Add TODO to use lifecycle components nodes to trigger initialization

Return a reaction result instead of bool on react()

Set invalidation flag to false on reset() in ForwardTrajectory local solver

Return local planner feedback from trajectory operator function calls

Fix segfault caused by passing through the global trajectory

Update comment, unify logging and add missing config parameters

Update to rolling
…eit#585)

Fix hybrid planning include folders (moveit#675)

Order stuff in the CMakeLists.txt and remove control_box package

Update README

Move member initialization to initializer lists

Remove control_box include dependency

Replace "loop-run" with "iteration"

Remove cpp interface class constructors and destructors

Use joint_state_broadcaster, clean up test node, add execution dependencies

Use only plugin interface header files and add missing dependencies

Clean up constructor and destructor definitions

Declare missing parameter in moveit_planning_pipeline_plugin

Move rclcpp::Loggers into anonymous namespaces

Switch CI branches to feature/hybrid_planning

Update message name

Remove moveit_msgs from .repos file

Update github workflows

Remove note from readme about building from source

Minor renamings, make reset() noexcept

Check for vector length of zero

Load moveit_cpp options with the Options helper. Reduces LOC.

Set the planning parameters within plan()

Function renaming

Authors and descriptions in header files only. New header file for error code interface.

Update namespacing

Use default QoS for subscribers

Better progress comparison

Add publish_joint_positions and publish_joint_velocities param

Grammar and other minor nitpicks

Restore moveit_msgs to .repos, for now
Refactor Global and Local Planner Components into NodeClasses

Add a simple launch test (#1)

Try to fix plugin export; add helpful debug when stuck

Error if global planning fails

READY and AWAIT_TRAJ states are redundant

Lock the planning scene as briefly as possible

Specify joint group when checking waypoint distance

Implement a reset() function in the local planner

Detect when the local planner gets stuck
Update robot state properly; remove debug prints; clang format

Check if the local planner is stuck within the forward_traj plugin, not local_planner_component
* Add unsuccessful action Hybrid Planning events and handle them in logic

* Replace std::once with simple bool variable

* Remove unneeded variable and update comments

// 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 =
Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

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

This line causes a segfault

Copy link
Copy Markdown

@tylerjw tylerjw Nov 30, 2021

Choose a reason for hiding this comment

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

Where is the pointer hybrid_planning_goal_handle_ initialized?

@sjahr sjahr force-pushed the pr-refactor_hybrid_planning_manager branch from daf4e37 to 3c4ac31 Compare November 30, 2021 10:56
SinglePlanExecution() = default;
~SinglePlanExecution() = default;
bool initialize(const std::shared_ptr<HybridPlanningManager>& hybrid_planning_manager) override;
bool initialize(const std::shared_ptr<HybridPlannerInterface>& hybrid_planner_interface) override;
Copy link
Copy Markdown

Choose a reason for hiding this comment

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

Locally I tried to solve the circular relationship here by giving a non-owning raw pointer to HybridPlanningManager through this interface.

The problem before is that these PlannerLogicInterface classes had a shared pointer to HybridPlanningManager which owned a list of PlannerLogicInterface objects. What this means is that that they share ownership of each other causing problems when they are destroyed (causing that bad weak ptr exception). The reason this was not caught before this change is we created the shared pointer to HybridPlannerInterface in a thread (timer callback) so it was created after HybridPlannerInterface finished initializing.

{
planner_logic_instance_ = planner_logic_plugin_loader_->createUniqueInstance(logic_plugin_name);
if (!planner_logic_instance_->initialize(HybridPlanningManager::shared_from_this()))
if (!planner_logic_instance_->initialize(hybrid_planner_interface_))
Copy link
Copy Markdown

Choose a reason for hiding this comment

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

Is hybrid_planner_interface_ initialized at this point? It looks to me that it isn't initialized until line 87.

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

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

Yes :/ An uninitialized pointer is passed to the planner logic instance with the promise, that it won't be needed before it is initialized. This problem with the circular lifetime ownership I have not solved yet but I think it is better than passing the whole hybrid planning manager

@AndyZe AndyZe force-pushed the feature/hybrid_planning branch 10 times, most recently from 43d89fd to 5cd1a5f Compare December 7, 2021 21:14
@sjahr sjahr closed this Apr 19, 2022
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

4 participants