Merge pull request #1033 from ubi-agni/fix-868

Fix #868: calling service capability locks PlanningSceneMonitor

MIGRATION.md

@@ -13,4 +13,6 @@ API changes in MoveIt! releases
 - In the C++ MoveGroupInterface class the ``plan()`` method returns a ``MoveItErrorCode`` object and not a boolean.
   `static_cast<bool>(mgi.plan())` can be used to achieve the old behavior.
 - ``CurrentStateMonitor::waitForCurrentState(double wait_time)`` has been renamed to ``waitForCompleteState`` to better reflect the actual semantics. Additionally a new method ``waitForCurrentState(const ros::Time t = ros::Time::now())`` was added that actually waits until all joint updates are newer than ``t``.
-
+- To avoid deadlocks, the PlanningSceneMonitor listens to its own EventQueue, monitored by an additional spinner thread.
+  Providing a custom NodeHandle, a user can control which EventQueue and processing thread is used instead.
+  Providing a default NodeHandle, the old behavior (using the global EventQueue) can be restored, which is however not recommended.

moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp

@@ -53,8 +53,8 @@ bool move_group::MoveGroupPlanService::computePlanService(moveit_msgs::GetMotion
 {
   ROS_INFO("Received new planning service request...");
   // before we start planning, ensure that we have the latest robot state received...
-  // for now disable this (see https://github.com/ros-planning/moveit/issues/868)
-  // context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
+  if (req.motion_plan_request.start_state.is_diff == true)
+    context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
   context_->planning_scene_monitor_->updateFrameTransforms();
 
   planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_);

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h

@@ -38,6 +38,7 @@
 #define MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_
 
 #include <ros/ros.h>
+#include <ros/callback_queue.h>
 #include <tf2_ros/message_filter.h>
 #include <tf2_ros/buffer.h>
 #include <message_filters/subscriber.h>
@@ -143,6 +144,20 @@ class PlanningSceneMonitor : private boost::noncopyable
                        const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
                        const std::string& name = "");
 
+  /** @brief Constructor
+   *  @param scene The scene instance to maintain up to date with monitored information
+   *  @param rml A pointer to a kinematic model loader
+   *  @param nh external parent NodeHandle
+   *         The monitors will use this NodeHandle's CallbackQueue for updates.
+   *         Usually, this should be a different queue than the global queue, otherwise you might run into timeouts.
+   *  @param tf_buffer A pointer to a tf2_ros::Buffer
+   *  @param name A name identifying this planning scene monitor
+   */
+  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
+                       const robot_model_loader::RobotModelLoaderPtr& rml, const ros::NodeHandle& nh,
+                       const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
+                       const std::string& name = "");
+
   ~PlanningSceneMonitor();
 
   /** \brief Get the name of this monitor */
@@ -450,6 +465,8 @@ class PlanningSceneMonitor : private boost::noncopyable
 
   ros::NodeHandle nh_;
   ros::NodeHandle root_nh_;
+  ros::CallbackQueue queue_;
+  std::shared_ptr<ros::AsyncSpinner> spinner_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
 
   std::string robot_description_;

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -120,35 +120,45 @@ const std::string planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNI
 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(const std::string& robot_description,
                                                                    const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
                                                                    const std::string& name)
-  : monitor_name_(name), nh_("~"), tf_buffer_(tf_buffer)
+  : PlanningSceneMonitor(planning_scene::PlanningScenePtr(), robot_description, tf_buffer, name)
 {
-  rm_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
-  initialize(planning_scene::PlanningScenePtr());
 }
 
 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
                                                                    const std::string& robot_description,
                                                                    const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
                                                                    const std::string& name)
-  : monitor_name_(name), nh_("~"), tf_buffer_(tf_buffer)
+  : PlanningSceneMonitor(scene, std::make_shared<robot_model_loader::RobotModelLoader>(robot_description), tf_buffer,
+                         name)
 {
-  rm_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
-  initialize(scene);
 }
 
 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(
     const robot_model_loader::RobotModelLoaderPtr& rm_loader, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
     const std::string& name)
-  : monitor_name_(name), nh_("~"), tf_buffer_(tf_buffer), rm_loader_(rm_loader)
+  : PlanningSceneMonitor(planning_scene::PlanningScenePtr(), rm_loader, tf_buffer, name)
 {
-  initialize(planning_scene::PlanningScenePtr());
 }
 
 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(
     const planning_scene::PlanningScenePtr& scene, const robot_model_loader::RobotModelLoaderPtr& rm_loader,
     const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
   : monitor_name_(name), nh_("~"), tf_buffer_(tf_buffer), rm_loader_(rm_loader)
 {
+  root_nh_.setCallbackQueue(&queue_);
+  nh_.setCallbackQueue(&queue_);
+  spinner_.reset(new ros::AsyncSpinner(1, &queue_));
+  spinner_->start();
+  initialize(scene);
+}
+
+planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(
+    const planning_scene::PlanningScenePtr& scene, const robot_model_loader::RobotModelLoaderPtr& rm_loader,
+    const ros::NodeHandle& nh, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
+  : monitor_name_(name), nh_("~"), root_nh_(nh), tf_buffer_(tf_buffer), rm_loader_(rm_loader)
+{
+  // use same callback queue as root_nh_
+  nh_.setCallbackQueue(root_nh_.getCallbackQueue());
   initialize(scene);
 }
 
@@ -164,6 +174,7 @@ planning_scene_monitor::PlanningSceneMonitor::~PlanningSceneMonitor()
   stopWorldGeometryMonitor();
   stopSceneMonitor();
 
+  spinner_.reset();
   delete reconfigure_impl_;
   current_state_monitor_.reset();
   scene_const_.reset();