From a696127d4595a3fa5af41fbfd803362c3a13eb62 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 8 Aug 2024 12:01:26 -0500 Subject: [PATCH] Optionally block in `getCurrentRobotState()` --- .../demos/cpp_interface/demo_joint_jog.cpp | 2 +- .../demos/cpp_interface/demo_pose.cpp | 2 +- .../demos/cpp_interface/demo_twist.cpp | 2 +- .../moveit_servo/include/moveit_servo/servo.hpp | 3 ++- moveit_ros/moveit_servo/src/servo.cpp | 15 ++++++++++++++- moveit_ros/moveit_servo/src/servo_node.cpp | 7 +++---- 6 files changed, 22 insertions(+), 9 deletions(-) diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp index 0ea73ec9cd..0d1315f6dc 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp +++ b/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 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()); diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp index aa71bda8b1..4c713fd8b6 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp +++ b/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 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; diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp index cd310b596e..eb816754eb 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp +++ b/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 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()); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index d50c831f98..5b8cb376d9 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/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. diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index d0ac00652a..e27b2256d0 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -643,8 +643,21 @@ std::optional 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); } diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index f5f7e137fb..65955d0f67 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -152,12 +152,11 @@ void ServoNode::pauseServo(const std::shared_ptrgetCurrentRobotState(); + 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; }