moveit#592 planning, planning_interface: updated according to PR move…

…it#702
alainsanguinetti · Jul 4, 2016 · 71ff802 · 71ff802
1 parent ef55f65
commit 71ff802
Show file tree

Hide file tree

Showing 8 changed files with 48 additions and 31 deletions.
diff --git a/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp
@@ -76,9 +76,12 @@ class KinematicsPluginLoader::KinematicsLoaderImpl
     }
   }
 
-  ~KinematicsLoaderImpl( ) {
+  /**
+   * \brief Explicit destructor to avoid bugs in unloading the library
+   */
+  ~KinematicsLoaderImpl( )
+  {
       kinematics_loader_.reset(); // we delete it manually
-      ROS_DEBUG( "KinematicsLoaderImpl destructor");
   }
 
   /**
@@ -250,7 +253,6 @@ void kinematics_plugin_loader::KinematicsPluginLoader::status() const
 
 kinematics_plugin_loader::KinematicsPluginLoader::~KinematicsPluginLoader()
 {
-    ROS_DEBUG( "KinematicsPluginLoader destructor" );
 }
 
 robot_model::SolverAllocatorFn kinematics_plugin_loader::KinematicsPluginLoader::getLoaderFunction()

diff --git a/...ning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h b/...ning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h
@@ -59,8 +59,10 @@ class CurrentStateMonitor
   /** @brief Constructor
    *  @param robot_model The current kinematic model to build on
    *  @param tf A pointer to the tf transformer to use
+   *  @param nh An optionnal node handle to pass options
    */
-  CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr<tf::Transformer> &tf);
+  CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr<tf::Transformer> &tf,
+        ros::NodeHandle nh = ros::NodeHandle() );
 
   ~CurrentStateMonitor();
 

diff --git a/planning/planning_scene_monitor/src/current_state_monitor.cpp b/planning/planning_scene_monitor/src/current_state_monitor.cpp
@@ -38,8 +38,10 @@
 #include <tf_conversions/tf_eigen.h>
 #include <limits>
 
-planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr<tf::Transformer> &tf)
-  : tf_(tf)
+planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr<tf::Transformer> &tf,
+                                                                 ros::NodeHandle nh )
+  : nh_( nh )
+  , tf_(tf)
   , robot_model_(robot_model)
   , robot_state_(robot_model)
   , state_monitor_started_(false)
@@ -51,8 +53,6 @@ planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_mod
 planning_scene_monitor::CurrentStateMonitor::~CurrentStateMonitor()
 {
   stopStateMonitor();
-
-  ROS_DEBUG( "CurrentStateMonitor destructor finished." );
 }
 
 robot_state::RobotStatePtr planning_scene_monitor::CurrentStateMonitor::getCurrentState() const

diff --git a/planning/robot_model_loader/src/robot_model_loader.cpp b/planning/robot_model_loader/src/robot_model_loader.cpp
@@ -51,12 +51,11 @@ robot_model_loader::RobotModelLoader::RobotModelLoader(const Options &opt)
   configure(opt);
 }
 
-robot_model_loader::RobotModelLoader::~RobotModelLoader() {
+robot_model_loader::RobotModelLoader::~RobotModelLoader()
+{
   model_.reset();
   rdf_loader_.reset();
   kinematics_loader_.reset();
-
-  ROS_DEBUG( "Robot model loader destructor" );
 }
 
 namespace

diff --git a/...nning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h b/...nning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h
@@ -49,7 +49,16 @@ boost::shared_ptr<tf::Transformer> getSharedTF();
 
 robot_model::RobotModelConstPtr getSharedRobotModel(const std::string &robot_description);
 
-planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf);
+/**
+ * @brief getSharedStateMonitor
+ *
+ * @param kmodel
+ * @param tf
+ * @param nh An optionnal node handle to pass options
+ * @return
+ */
+planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf,
+    ros::NodeHandle nh = ros::NodeHandle() );
 
 }
 }

diff --git a/planning_interface/common_planning_interface_objects/src/common_objects.cpp b/planning_interface/common_planning_interface_objects/src/common_objects.cpp
@@ -48,17 +48,9 @@ struct SharedStorage
 
   ~SharedStorage()
   {
-    ROS_DEBUG( "Shared storage destructor begins" );
     tf_.reset();
-    ROS_DEBUG( "TF was reset" );
-
     state_monitors_.clear();
-    ROS_DEBUG( "state_monitors_ was cleared" );
-
     model_loaders_.clear();
-    ROS_DEBUG( "model_loaders_ was cleared" );
-
-    ROS_DEBUG( "Shared storage destructor finished" );
   }
 
   boost::mutex lock_;
@@ -104,15 +96,16 @@ robot_model::RobotModelConstPtr getSharedRobotModel(const std::string &robot_des
   }
 }
 
-planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf)
+planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf,
+    ros::NodeHandle nh )
 {
   SharedStorage &s = getSharedStorage();
   boost::mutex::scoped_lock slock(s.lock_);
   if (s.state_monitors_.find(kmodel->getName()) != s.state_monitors_.end())
     return s.state_monitors_[kmodel->getName()];
   else
   {
-    planning_scene_monitor::CurrentStateMonitorPtr monitor(new planning_scene_monitor::CurrentStateMonitor(kmodel, tf));
+    planning_scene_monitor::CurrentStateMonitorPtr monitor(new planning_scene_monitor::CurrentStateMonitor(kmodel, tf, nh ));
     s.state_monitors_[kmodel->getName()] = monitor;
     return monitor;
   }

diff --git a/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h b/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h
@@ -112,7 +112,10 @@ class MoveGroup
 
   /** \brief Construct a client for the MoveGroup action using a specified set of options \e opt. Optionally, specify a TF instance to use.
       If not specified, one will be constructed internally. A timeout for connecting to the action server can also be specified. If it is not specified,
-      the wait time is unlimited. */
+      the wait time is unlimited.
+    *
+    * @param opt. An option structure, if you pass a node with a specific callback queue, it has to of type ros::CallbackQueue.
+    */
   MoveGroup(const Options &opt, const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
             const ros::Duration &wait_for_server = ros::Duration(0, 0));
 

diff --git a/planning_interface/move_group_interface/src/move_group.cpp b/planning_interface/move_group_interface/src/move_group.cpp
@@ -128,7 +128,7 @@ class MoveGroup::MoveGroupImpl
     trajectory_event_publisher_ = node_handle_.advertise<std_msgs::String>(trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false);
     attached_object_publisher_ = node_handle_.advertise<moveit_msgs::AttachedCollisionObject>(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1, false);
 
-    current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_);
+    current_state_monitor_ = getSharedStateMonitor( robot_model_, tf_, node_handle_ );
 
     move_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>(node_handle_,
                                                                                               move_group::MOVE_ACTION,
@@ -161,26 +161,35 @@ class MoveGroup::MoveGroupImpl
     ros::Time start_time = ros::Time::now();
     while (start_time == ros::Time::now())
     {
-      ros::WallDuration(0.01).sleep();
+      ros::WallDuration(0.001).sleep();
       ros::spinOnce();
+      if( node_handle_.getCallbackQueue() ) {
+        ( ( ros::CallbackQueue * ) node_handle_.getCallbackQueue())->callAvailable();
+      }
     }
 
     // wait for the server (and spin as needed)
     if (wait_for_server == ros::Duration(0, 0))
     {
       while (node_handle_.ok() && !action->isServerConnected())
       {
-        ros::WallDuration(0.02).sleep();
+        ros::WallDuration(0.001).sleep();
         ros::spinOnce();
+        if( node_handle_.getCallbackQueue() ) {
+          ( ( ros::CallbackQueue * ) node_handle_.getCallbackQueue())->callAvailable();
+        }
       }
     }
     else
     {
       ros::Time final_time = ros::Time::now() + wait_for_server;
       while (node_handle_.ok() && !action->isServerConnected() && final_time > ros::Time::now())
       {
-        ros::WallDuration(0.02).sleep();
+        ros::WallDuration(0.001).sleep();
         ros::spinOnce();
+        if( node_handle_.getCallbackQueue() ) {
+          ( ( ros::CallbackQueue * ) node_handle_.getCallbackQueue())->callAvailable();
+        }
       }
     }
 
@@ -195,11 +204,10 @@ class MoveGroup::MoveGroupImpl
     }
   }
 
-  ~MoveGroupImpl() {
-    ROS_DEBUG( "MoveGroupImpl destructor begins" );
+  ~MoveGroupImpl()
+  {
     if (constraints_init_thread_)
       constraints_init_thread_->join();
-    ROS_DEBUG( "MoveGroupImpl destructor finished" );
   }
 
   const boost::shared_ptr<tf::Transformer>& getTF() const
@@ -1097,7 +1105,8 @@ moveit::planning_interface::MoveGroup::~MoveGroup() {
   ROS_DEBUG( "MoveGroup destructor finished" );
 }
 
-const std::string& moveit::planning_interface::MoveGroup::getName() const {
+const std::string& moveit::planning_interface::MoveGroup::getName() const
+{
   return impl_->getOptions().group_name_;
 }