Ensure the robot state is up-to-date before Servoing (#2954)

* Ensure the robot state is up-to-date before Servoing

* Optionally block in `getCurrentRobotState()`

* Always reset the smoothing filter when paused

moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp

@@ -95,7 +95,7 @@ int main(int argc, char* argv[])
 
   // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
-  KinematicState current_state = servo.getCurrentRobotState();
+  KinematicState current_state = servo.getCurrentRobotState(true /* wait for updated state */);
   updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());

moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp

@@ -119,7 +119,7 @@ int main(int argc, char* argv[])
 
   // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
-  KinematicState current_state = servo.getCurrentRobotState();
+  KinematicState current_state = servo.getCurrentRobotState(true /* wait for updated state */);
   updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
 
   bool satisfies_linear_tolerance = false;

moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp

@@ -98,7 +98,7 @@ int main(int argc, char* argv[])
 
   // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
-  KinematicState current_state = servo.getCurrentRobotState();
+  KinematicState current_state = servo.getCurrentRobotState(true /* wait for updated state */);
   updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -118,9 +118,10 @@ class Servo
 
   /**
    * \brief Get the current state of the robot as given by planning scene monitor.
+   * @param block_for_current_state If true, we explicitly wait for a new robot state
    * @return The current state of the robot.
    */
-  KinematicState getCurrentRobotState() const;
+  KinematicState getCurrentRobotState(bool block_for_current_state) const;
 
   /**
    * \brief Smoothly halt at a commanded state when command goes stale.

moveit_ros/moveit_servo/src/servo.cpp

@@ -158,7 +158,6 @@ void Servo::setSmoothingPlugin()
     RCLCPP_ERROR(logger_, "Smoothing plugin could not be initialized");
     std::exit(EXIT_FAILURE);
   }
-  resetSmoothing(getCurrentRobotState());
 }
 
 void Servo::doSmoothing(KinematicState& state)
@@ -526,9 +525,6 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot
     // Adjust joint position based on scaled down velocity
     target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period);
 
-    // Apply smoothing to the positions if a smoother was provided.
-    doSmoothing(target_state);
-
     // Apply collision scaling to the joint position delta
     target_state.positions =
         current_state.positions + collision_velocity_scale_ * (target_state.positions - current_state.positions);
@@ -548,8 +544,8 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot
     }
   }
 
-  // Update internal state of filter with final calculated command.
-  resetSmoothing(target_state);
+  // Apply smoothing to the positions if a smoother was provided.
+  doSmoothing(target_state);
 
   return target_state;
 }
@@ -647,8 +643,21 @@ std::optional<PoseCommand> Servo::toPlanningFrame(const PoseCommand& command, co
   return PoseCommand{ planning_frame, planning_to_command_tf * command.pose };
 }
 
-KinematicState Servo::getCurrentRobotState() const
+KinematicState Servo::getCurrentRobotState(bool block_for_current_state) const
 {
+  if (block_for_current_state)
+  {
+    bool have_current_state = false;
+    while (rclcpp::ok() && !have_current_state)
+    {
+      have_current_state = planning_scene_monitor_->getStateMonitor()->waitForCurrentState(
+          rclcpp::Clock(RCL_ROS_TIME).now(), ROBOT_STATE_WAIT_TIME /* s */);
+      if (!have_current_state)
+      {
+        RCLCPP_WARN(logger_, "Waiting for the current state");
+      }
+    }
+  }
   moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
   return extractRobotState(robot_state, servo_params_.move_group_name);
 }
@@ -665,9 +674,6 @@ std::pair<bool, KinematicState> Servo::smoothHalt(const KinematicState& halt_sta
     // set target velocity
     target_state.velocities *= 0.0;
 
-    // apply smoothing: this will change target position/velocity to make slow down gradual
-    doSmoothing(target_state);
-
     // scale velocity in case of obstacle
     target_state.velocities *= collision_velocity_scale_;
 
@@ -681,7 +687,9 @@ std::pair<bool, KinematicState> Servo::smoothHalt(const KinematicState& halt_sta
     }
   }
 
-  resetSmoothing(target_state);
+  // apply smoothing: this will change target position/velocity to make slow down gradual
+  doSmoothing(target_state);
+
   return std::make_pair(stopped, target_state);
 }
 

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -152,12 +152,11 @@ void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request
   else
   {
     // Reset the smoothing plugin with the robot's current state in case the robot moved between pausing and unpausing.
-    last_commanded_state_ = servo_->getCurrentRobotState();
+    last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */);
     servo_->resetSmoothing(last_commanded_state_);
 
     // clear out the command rolling window and reset last commanded state to be the current state
     joint_cmd_rolling_window_.clear();
-    last_commanded_state_ = servo_->getCurrentRobotState();
 
     // reactivate collision checking
     servo_->setCollisionChecking(true);
@@ -301,8 +300,10 @@ void ServoNode::servoLoop()
     RCLCPP_INFO(node_->get_logger(), "Waiting to receive robot state update.");
     rclcpp::sleep_for(std::chrono::seconds(1));
   }
-  KinematicState current_state = servo_->getCurrentRobotState();
+  KinematicState current_state = servo_->getCurrentRobotState(true /* block for current robot state */);
   last_commanded_state_ = current_state;
+  // Ensure the filter is up to date
+  servo_->resetSmoothing(current_state);
 
   // Get the robot state and joint model group info.
   moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
@@ -314,6 +315,7 @@ void ServoNode::servoLoop()
     // Skip processing if servoing is disabled.
     if (servo_paused_)
     {
+      servo_->resetSmoothing(current_state);
       servo_frequency.sleep();
       continue;
     }
@@ -329,7 +331,7 @@ void ServoNode::servoLoop()
     {
       // if all joint_cmd_rolling_window_ is empty or all commands in it are outdated, use current robot state
       joint_cmd_rolling_window_.clear();
-      current_state = servo_->getCurrentRobotState();
+      current_state = servo_->getCurrentRobotState(false /* block for current robot state */);
       current_state.velocities *= 0.0;
     }