Skip to content

Remove timer init from HybridPlanningManager#4

Open
tylerjw wants to merge 12 commits intoAndyZe:feature/hybrid_planningfrom
tylerjw:hybrid_init
Open

Remove timer init from HybridPlanningManager#4
tylerjw wants to merge 12 commits intoAndyZe:feature/hybrid_planningfrom
tylerjw:hybrid_init

Conversation

@tylerjw
Copy link
Copy Markdown

@tylerjw tylerjw commented Nov 29, 2021

Description

Here are the changes I asked about in the main PR.

sjahr and others added 11 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
@AndyZe
Copy link
Copy Markdown
Owner

AndyZe commented Nov 29, 2021

Sorry to say that I got this:


[hybrid_planning_demo_node-5] [INFO] [1638225773.375163130] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00822692 seconds
[hybrid_planning_demo_node-5] [INFO] [1638225773.375260555] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
[hybrid_planning_demo_node-5] [WARN] [1638225773.406684737] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!
[hybrid_planning_demo_node-5] [INFO] [1638225773.419255884] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[hybrid_planning_demo_node-5] [INFO] [1638225773.419583843] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[hybrid_planning_demo_node-5] [INFO] [1638225773.420874813] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on '/planning_scene'
[hybrid_planning_demo_node-5] [INFO] [1638225773.420971549] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[hybrid_planning_demo_node-5] [INFO] [1638225773.421671851] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[hybrid_planning_demo_node-5] [ERROR] [1638225793.422533534] [test_hybrid_planning_client]: Hybrid planning action server not available after waiting

@AndyZe
Copy link
Copy Markdown
Owner

AndyZe commented Nov 29, 2021

Test with: ros2 launch moveit_hybrid_planning hybrid_planning_demo.launch.py

@tylerjw
Copy link
Copy Markdown
Author

tylerjw commented Nov 29, 2021

I just searched for this line:

[hybrid_planning_demo_node-5] [ERROR] [1638225793.422533534] [test_hybrid_planning_client]: Hybrid planning action server not available after waiting

And found this code:

if (!hp_action_client_->wait_for_action_server(20s))
{
   RCLCPP_ERROR(LOGGER, "Hybrid planning action server not available after waiting");
   return;
}

Waiting on a timer is a recipe for a flaky test and there is a bunch of these. I'll try to troubleshoot this and make a note to fix this in the future. Are there any other errors?

@tylerjw
Copy link
Copy Markdown
Author

tylerjw commented Nov 29, 2021

I just noticed [hybrid_planning_demo_node-5] [WARN] [1638225773.406684737] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!, is that normal?

@tylerjw
Copy link
Copy Markdown
Author

tylerjw commented Nov 29, 2021

Running the integration test locally I get this error:

1: [component_container_mt-1] [ERROR] [1638229085.547834900] [hybrid_planning_container]: Component constructor threw an exception: bad_weak_ptr
1: [ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'hybrid_planning_manager' of type 'moveit::hybrid_planning::HybridPlanningManager' in container '/hybrid_planning_container': Component constructor threw an exception: bad_weak_ptr
1: [ros2_control_node-6] [INFO] [1638229085.549128910] [panda_joint_group_position_controller]: configure successful
1: [ros2 run controller_manager spawner panda_joint_group_position_controller-8] [INFO] [1638229085.552789766] [spawner_panda_joint_group_position_controller]: Configured and started panda_joint_group_position_controller
1: [INFO] [ros2 run controller_manager spawner panda_joint_group_position_controller-8]: process has finished cleanly [pid 960103]

@tylerjw
Copy link
Copy Markdown
Author

tylerjw commented Nov 30, 2021

@sjahr here is my attempted fix. Note that this fixes the circular lifetime ownership relationship by handing a non-owning pointer to the plugins. I believe this is safe but now I'm struggling with the way parameters are handled (all the string ones default to <undefined> but then we check for an empty string for it not being set and I can't figure out why the parameter for moveit_controller_manager is not being read from the yaml file).

@AndyZe AndyZe force-pushed the feature/hybrid_planning branch 10 times, most recently from 43d89fd to 5cd1a5f Compare December 7, 2021 21:14
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.

5 participants