Merge pull request #724 from ros-planning/fixup-#716

fixup #716

move_group/src/default_capabilities/move_action_capability.cpp

@@ -61,6 +61,7 @@ void move_group::MoveGroupMoveAction::initialize()
 void move_group::MoveGroupMoveAction::executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr& goal)
 {
   setMoveState(PLANNING);
+  context_->planning_scene_monitor_->updateFrameTransforms();
 
   moveit_msgs::MoveGroupResult action_res;
   if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
@@ -170,6 +171,7 @@ bool move_group::MoveGroupMoveAction::planUsingPlanningPipeline(const planning_i
 {
   setMoveState(PLANNING);
 
+  planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_);
   bool solved = false;
   planning_interface::MotionPlanResponse res;
   try

move_group/src/default_capabilities/plan_service_capability.cpp

@@ -52,6 +52,7 @@ bool move_group::MoveGroupPlanService::computePlanService(moveit_msgs::GetMotion
 {
   ROS_INFO("Received new planning service request...");
   context_->planning_scene_monitor_->syncSceneUpdates();
+  context_->planning_scene_monitor_->updateFrameTransforms();
 
   bool solved = false;
   planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_);

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

@@ -521,7 +521,6 @@ class PlanningSceneMonitor : private boost::noncopyable
   ros::CallbackQueue                    callback_queue_;
   boost::scoped_ptr<ros::AsyncSpinner>  spinner_;
   ros::Time                             last_robot_motion_time_; /// Last time the robot has moved
-  bool                                  enforce_next_state_update_;
 };
 
 typedef boost::shared_ptr<PlanningSceneMonitor> PlanningSceneMonitorPtr;

planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -232,7 +232,6 @@ void planning_scene_monitor::PlanningSceneMonitor::initialize(const planning_sce
   shape_transform_cache_lookup_wait_time_ = ros::Duration(temp_wait_time);
 
   state_update_pending_ = false;
-  enforce_next_state_update_ = false;
   state_update_timer_ = nh_.createWallTimer(dt_state_update_,
                                             &PlanningSceneMonitor::stateUpdateTimerCallback,
                                             this,
@@ -824,8 +823,6 @@ void planning_scene_monitor::PlanningSceneMonitor::syncSceneUpdates(const ros::T
   if (t.isZero())
     return;
 
-  enforce_next_state_update_ = true;  // enforce next state update to trigger without throttling
-
   // Robot state updates in the scene are only triggered by the state monitor on changes of the state.
   // Hence, last_state_update_time_ might be much older than current_state_monitor_ (when robot didn't moved for a while).
   boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
@@ -834,15 +831,21 @@ void planning_scene_monitor::PlanningSceneMonitor::syncSceneUpdates(const ros::T
          last_robot_update < t &&                      // wait for recent state update
          (t - last_robot_motion_time_).toSec() < 1.0)  // but only if robot moved in last second
   {
-    new_scene_update_condition_.wait_for(lock, boost::chrono::milliseconds(50));
+    new_scene_update_condition_.wait_for(lock, boost::chrono::milliseconds(100));
     last_robot_update = current_state_monitor_->getCurrentStateTime();
   }
-  // Now, we know that robot state is up-to-date
+  // If there was a state monitor connected (and robot moved), the robot state should be up-to-date now
+  // and last_update_time_ = last_robot_motion_time_
+
+  // If last scene update is recent and there are no pending updates, we are done.
+  if (last_update_time_ >= t && callback_queue_.empty())
+    return;
 
-  // ensure that last update time is more recent than t (or no more update events pending)
-  while (last_update_time_ < t && !callback_queue_.empty())
+  // Processing pending updates and wait for new incoming updates up to 1s.
+  // This is necessary as publishing planning scene diffs is throttled (2Hz by default).
+  while (!callback_queue_.empty() || (ros::Time::now()-t).toSec() < 1.)
   {
-    new_scene_update_condition_.wait_for(lock, boost::chrono::milliseconds(50));
+    new_scene_update_condition_.wait_for(lock, boost::chrono::seconds(1));
   }
 }
 
@@ -1058,11 +1061,11 @@ void planning_scene_monitor::PlanningSceneMonitor::onStateUpdate(const sensor_ms
   const ros::WallTime &n = ros::WallTime::now();
   ros::WallDuration dt = n - wall_last_state_update_;
 
-  bool update = enforce_next_state_update_;
+  bool update = false;
   {
     boost::mutex::scoped_lock lock(state_pending_mutex_);
 
-    if (dt < dt_state_update_ && !update)
+    if (dt < dt_state_update_)
     {
       state_update_pending_ = true;
     }
@@ -1168,7 +1171,6 @@ void planning_scene_monitor::PlanningSceneMonitor::updateSceneWithCurrentState()
       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
       last_update_time_ = last_robot_motion_time_ = current_state_monitor_->getCurrentStateTime();
       current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
-      enforce_next_state_update_ = false;
       scene_->getCurrentStateNonConst().update(); // compute all transforms
     }
     triggerSceneUpdateEvent(UPDATE_STATE);

planning_interface/move_group_interface/src/move_group.cpp

@@ -709,7 +709,7 @@ class MoveGroup::MoveGroupImpl
         return false;
       }
       // TODO: check multi-DoF joints
-      if (fabs(current_state->getJointPositions(jm)[0] - positions[i]) < std::numeric_limits<float>::epsilon())
+      if (fabs(current_state->getJointPositions(jm)[0] - positions[i]) > std::numeric_limits<float>::epsilon())
       {
         ROS_ERROR("Trajectory start deviates from current robot state");
         return false;

visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp

@@ -406,11 +406,10 @@ void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::EmptyCo
   if (move_group_ && planning_display_)
   {
     planning_display_->syncSceneUpdates();
-    robot_state::RobotState state = *planning_display_->getQueryStartState();
     const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO();
     if (ps)
     {
-      state = ps->getCurrentState();
+      robot_state::RobotState state = ps->getCurrentState();
       planning_display_->setQueryStartState(state);
     }
   }
@@ -421,11 +420,10 @@ void MotionPlanningFrame::remoteUpdateGoalStateCallback(const std_msgs::EmptyCon
   if (move_group_ && planning_display_)
   {
     planning_display_->syncSceneUpdates();
-    robot_state::RobotState state = *planning_display_->getQueryStartState();
     const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO();
     if (ps)
     {
-      state = ps->getCurrentState();
+      robot_state::RobotState state = ps->getCurrentState();
       planning_display_->setQueryGoalState(state);
     }
   }