Optionally block in getCurrentRobotState()

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

@@ -643,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);
 }

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,7 +300,7 @@ 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);
@@ -331,7 +330,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;
     }