motion planning stuck after several calls.

[lianghongzhuo]

Description

Overview of your issue here.

Your environment

• ROS Distro: [Melodic]
• OS Version: Ubuntu 18.04
• Source or Binary build?
• If source, master

Steps to reproduce

self.hand_commander = MoveGroupCommander("hand")
self.hand_commander.set_start_state_to_current_state()
self.hand_commander.set_joint_value_target(hand_joint_positions)
self.hand_commander.go(wait=True)

Expected behaviour

Go to the target joint position

Actual behaviour

Code stuck at self.hand_commander.go(wait=True)

Backtrace or Console output

No error msg

My guess is this behaviour is related to #2663 which introduce a feature that The joint states of passivejoints must be published in ROS and the CurrentStateMonitor will now wait for them as well. If so, please provide a possible fix for a robot such as shadow hand.

[welcome]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[lianghongzhuo]

Update,
after check out to v1.1.4 the bug still exists, so not related to #2663

after check out to commit cdda16f
The bug disappears.
So I bet this bug triggered by #2596

[lianghongzhuo]

to reproduce: #2596 (comment)

[simonschmeisser]

I'm testing what you posted in the PR and can indeed reproduce a dead lock. Now I attached gdb to the move_group binary and can see that two threads are doing something with a lock:

thread # 6 processes an new point cloud and wants a write lock on the octomap

1   futex_wait_cancelable                                                                                                                                                                                                                                                                                                                                                     futex-internal.h               88  0x7fec6eb7dad3 
2   __pthread_cond_wait_common                                                                                                                                                                                                                                                                                                                                                pthread_cond_wait.c            502 0x7fec6eb7dad3 
3   __pthread_cond_wait                                                                                                                                                                                                                                                                                                                                                       pthread_cond_wait.c            655 0x7fec6eb7dad3 
4   boost::condition_variable::wait                                                                                                                                                                                                                                                                                                                                           condition_variable.hpp         81  0x7fec6f022b7d 
5   boost::shared_mutex::lock                                                                                                                                                                                                                                                                                                                                                 shared_mutex.hpp               294 0x7fec6f022b7d 
6   collision_detection::OccMapTree::lockWrite                                                                                                                                                                                                                                                                                                                                occupancy_map.h                77  0x7fec506beb1c 
7   occupancy_map_monitor::PointCloudOctomapUpdater::cloudMsgCallback                                                                                                                                                                                                                                                                                                         pointcloud_octomap_updater.cpp 328 0x7fec506beb1c 
8   boost::function1<void, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void>> const> const&>::operator()                                                                                                                                                                                                                                                       function_template.hpp          759 0x7fec506ca96e 
9   boost::detail::function::void_function_obj_invoker1<boost::function<void (boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void>> const> const&)>, void, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void>> const>>::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void>> const>) function_template.hpp          159 0x7fec506ca96e 
10  boost::function1<void, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void>> const>>::operator()                                                                                                                                                                                                                                                              function_template.hpp          759 0x7fec506d2990 
11  message_filters::CallbackHelper1T<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void>> const> const&, sensor_msgs::PointCloud2_<std::allocator<void>>>::call                                                                                                                                                                                                 signal1.h                      76  0x7fec506d2990 
12  message_filters::Signal1<sensor_msgs::PointCloud2_<std::allocator<void>>>::call                                                                                                                                                                                                                                                                                           signal1.h                      119 0x7fec506cf1bf 
13  message_filters::SimpleFilter<sensor_msgs::PointCloud2_<std::allocator<void>>>::signalMessage                                                                                                                                                                                                                                                                             simple_filter.h                136 0x7fec506d49f1 
14  tf2_ros::MessageFilter<sensor_msgs::PointCloud2_<std::allocator<void>>>::CBQueueCallback::call                                                                                                                                                                                                                                                                            message_filter.h               596 0x7fec506d49f1 
15  ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS *)                                                                                                                                                                                                                                                                                                                                                     0x7fec6dfcad99 
16  ros::CallbackQueue::callAvailable(ros::WallDuration)                                                                                                                                                                                                                                                                                                                                                         0x7fec6dfccb0b 
17  ros::AsyncSpinnerImpl::threadFunc()                                                                                                                                                                                                                                                                                                                                                                          0x7fec6e023245 
18  ??                                                                                                                                                                                                                                                                                                                                                                                                           0x7fec69d59bcd 
19  start_thread                                                                                                                                                                                                                                                                                                                                                              pthread_create.c               463 0x7fec6eb776db 
20  clone                                                                                                                                                                                                                                                                                                                                                                     clone.S                        95  0x7fec6c9d771f 
... <more>                                                                                                                                                                                                                                                                                                                                                                                                                      

and thread # 14 is planning the path your python script requested

1  futex_wait_cancelable                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 futex-internal.h                     88  0x7fec6eb7dad3 
2  __pthread_cond_wait_common                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            pthread_cond_wait.c                  502 0x7fec6eb7dad3 
3  __pthread_cond_wait                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   pthread_cond_wait.c                  655 0x7fec6eb7dad3 
4  boost::condition_variable::wait                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       condition_variable.hpp               81  0x7fec6f02249d 
5  boost::shared_mutex::lock_shared                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      shared_mutex.hpp                     191 0x7fec6f02249d 
6  boost::shared_lock<boost::shared_mutex>::lock                                                                                                                                                                                                                                                                                                                                                                                                                                                                         lock_types.hpp                       645 0x7fec6f024550 
7  boost::shared_lock<boost::shared_mutex>::shared_lock                                                                                                                                                                                                                                                                                                                                                                                                                                                                  lock_types.hpp                       520 0x7fec6a9fd2c1 
8  collision_detection::OccMapTree::reading                                                                                                                                                                                                                                                                                                                                                                                                                                                                              occupancy_map.h                      91  0x7fec6a9fd2c1 
9  planning_scene::PlanningScene::checkCollision                                                                                                                                                                                                                                                                                                                                                                                                                                                                         planning_scene.cpp                   507 0x7fec6a9fd2c1 
10 planning_scene::PlanningScene::checkCollision                                                                                                                                                                                                                                                                                                                                                                                                                                                                         planning_scene.cpp                   483 0x7fec6a9fd345 
11 planning_scene::PlanningScene::checkCollision                                                                                                                                                                                                                                                                                                                                                                                                                                                                         planning_scene.h                     392 0x7fec335b019c 
12 ompl_interface::StateValidityChecker::isValid                                                                                                                                                                                                                                                                                                                                                                                                                                                                         state_validity_checker.cpp           110 0x7fec335b019c 
13 ompl::base::DiscreteMotionValidator::checkMotion(ompl::base::State const *, ompl::base::State const *) const                                                                                                                                                                                                                                                                                                                                                                                                                                                   0x7fec32cf80d5 
14 ompl::geometric::PathSimplifier::smoothBSpline(ompl::geometric::PathGeometric&, unsigned int, double)                                                                                                                                                                                                                                                                                                                                                                                                                                                          0x7fec32f041ef 
15 ompl::geometric::PathSimplifier::simplify(ompl::geometric::PathGeometric&, ompl::base::PlannerTerminationCondition const&, bool)                                                                                                                                                                                                                                                                                                                                                                                                                               0x7fec32f089d3 
16 ompl::geometric::PathSimplifier::simplify(ompl::geometric::PathGeometric&, double, bool)                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0x7fec32f08de0 
17 ompl::geometric::SimpleSetup::simplifySolution(double)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         0x7fec32f09d9a 
18 ompl_interface::ModelBasedPlanningContext::simplifySolution                                                                                                                                                                                                                                                                                                                                                                                                                                                           model_based_planning_context.cpp     402 0x7fec3358da40 
19 ompl_interface::ModelBasedPlanningContext::solve                                                                                                                                                                                                                                                                                                                                                                                                                                                                      model_based_planning_context.cpp     684 0x7fec33594244 
20 planning_request_adapter::(anonymous namespace)::callPlannerInterfaceSolve                                                                                                                                                                                                                                                                                                                                                                                                                                            planning_request_adapter.cpp         54  0x7fec67b482d7 
21 boost::function3<bool, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&>::operator()                                                                                                                                                                                                                                                                                                                function_template.hpp                760 0x7fec3242780f 
22 default_planner_request_adapters::FixStartStatePathConstraints::adaptAndPlan(boost::function<bool (std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long>>&) const fix_start_state_path_constraints.cpp 131 0x7fec3242780f 
23 planning_request_adapter::PlanningRequestAdapter::adaptAndPlan                                                                                                                                                                                                                                                                                                                                                                                                                                                        planning_request_adapter.cpp         67  0x7fec67b4816b 
24 planning_request_adapter::(anonymous namespace)::callAdapter1                                                                                                                                                                                                                                                                                                                                                                                                                                                         planning_request_adapter.cpp         90  0x7fec67b483de 
25 boost::function3<bool, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&>::operator()                                                                                                                                                                                                                                                                                                                function_template.hpp                760 0x7fec32420f45 
26 default_planner_request_adapters::FixStartStateCollision::adaptAndPlan(boost::function<bool (std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long>>&) const       fix_start_state_collision.cpp        184 0x7fec32420f45 
27 planning_request_adapter::(anonymous namespace)::callAdapter2                                                                                                                                                                                                                                                                                                                                                                                                                                                         planning_request_adapter.cpp         108 0x7fec67b486ef 
28 boost::function3<bool, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&>::operator()                                                                                                                                                                                                                                                                                                                function_template.hpp                760 0x7fec3241c970 
29 default_planner_request_adapters::FixStartStateBounds::adaptAndPlan(boost::function<bool (std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long>>&) const          fix_start_state_bounds.cpp           188 0x7fec3241c970 
30 planning_request_adapter::(anonymous namespace)::callAdapter2                                                                                                                                                                                                                                                                                                                                                                                                                                                         planning_request_adapter.cpp         108 0x7fec67b486ef 
31 boost::function3<bool, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&>::operator()                                                                                                                                                                                                                                                                                                                function_template.hpp                760 0x7fec3242a371 
32 default_planner_request_adapters::FixWorkspaceBounds::adaptAndPlan(boost::function<bool (std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long>>&) const           fix_workspace_bounds.cpp             84  0x7fec3242a371 
33 planning_request_adapter::(anonymous namespace)::callAdapter2                                                                                                                                                                                                                                                                                                                                                                                                                                                         planning_request_adapter.cpp         108 0x7fec67b486ef 
34 boost::function3<bool, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&>::operator()                                                                                                                                                                                                                                                                                                                function_template.hpp                760 0x7fec3242d15f 
35 default_planner_request_adapters::ResolveConstraintFrames::adaptAndPlan(boost::function<bool (std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long>>&) const      resolve_constraint_frames.cpp        69  0x7fec3242d15f 
36 planning_request_adapter::(anonymous namespace)::callAdapter2                                                                                                                                                                                                                                                                                                                                                                                                                                                         planning_request_adapter.cpp         108 0x7fec67b486ef 
37 boost::function3<bool, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&>::operator()                                                                                                                                                                                                                                                                                                                function_template.hpp                760 0x7fec3242a84e 
38 default_planner_request_adapters::AddTimeParameterization::adaptAndPlan(boost::function<bool (std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long>>&) const      add_time_parameterization.cpp        64  0x7fec3242a84e 
39 planning_request_adapter::(anonymous namespace)::callAdapter2                                                                                                                                                                                                                                                                                                                                                                                                                                                         planning_request_adapter.cpp         108 0x7fec67b486ef 
40 boost::function3<bool, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void>> const&, planning_interface::MotionPlanResponse&>::operator()                                                                                                                                                                                                                                                                                                                function_template.hpp                760 0x7fec67b4982a 
41 planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan                                                                                                                                                                                                                                                                                                                                                                                                                                                   planning_request_adapter.cpp         152 0x7fec67b4982a 
42 planning_pipeline::PlanningPipeline::generatePlan                                                                                                                                                                                                                                                                                                                                                                                                                                                                     planning_pipeline.cpp                237 0x7fec6c677ea8 
43 planning_pipeline::PlanningPipeline::generatePlan                                                                                                                                                                                                                                                                                                                                                                                                                                                                     planning_pipeline.cpp                213 0x7fec6c67975a 
44 move_group::MoveGroupMoveAction::planUsingPlanningPipeline                                                                                                                                                                                                                                                                                                                                                                                                                                                            move_action_capability.cpp           225 0x7fec31f0f6e8 
45 boost::function1<bool, plan_execution::ExecutableMotionPlan&>::operator()                                                                                                                                                                                                                                                                                                                                                                                                                                             function_template.hpp                760 0x7fec6c43af15 
46 plan_execution::PlanExecution::planAndExecuteHelper                                                                                                                                                                                                                                                                                                                                                                                                                                                                   plan_execution.cpp                   189 0x7fec6c43af15 
47 plan_execution::PlanExecution::planAndExecute                                                                                                                                                                                                                                                                                                                                                                                                                                                                         plan_execution.cpp                   138 0x7fec6c43bac5 
48 plan_execution::PlanExecution::planAndExecute                                                                                                                                                                                                                                                                                                                                                                                                                                                                         plan_execution.cpp                   145 0x7fec6c43bc0e 
49 move_group::MoveGroupMoveAction::executeMoveCallbackPlanAndExecute                                                                                                                                                                                                                                                                                                                                                                                                                                                    move_action_capability.cpp           155 0x7fec31f104db 
50 move_group::MoveGroupMoveAction::executeMoveCallback                                                                                                                                                                                                                                                                                                                                                                                                                                                                  move_action_capability.cpp           80  0x7fec31f10e52 
51 boost::function1<void, boost::shared_ptr<moveit_msgs::MoveGroupGoal_<std::allocator<void>> const> const&>::operator()                                                                                                                                                                                                                                                                                                                                                                                                 function_template.hpp                759 0x7fec31f24727 
52 actionlib::SimpleActionServer<moveit_msgs::MoveGroupAction_<std::allocator<void>>>::executeLoop                                                                                                                                                                                                                                                                                                                                                                                                                       simple_action_server_imp.h           385 0x7fec31f24727 
53 ??                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             0x7fec69d59bcd 
54 start_thread                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          pthread_create.c                     463 0x7fec6eb776db 
55 clone                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 clone.S                              95  0x7fec6c9d771f 

I do not understand yet why they are both waiting and why there is a dead lock unfortunately.

[aditya2592]

Hi, we are seeing a similar bug in moveit2. Was #2683 copied to moveit2 or is not needed there?