From d034acd121e7fc87bf749c8697f7b5b591c25010 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 12 Aug 2024 19:58:11 -0500 Subject: [PATCH] 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 --- .../demos/cpp_interface/demo_joint_jog.cpp | 2 +- .../demos/cpp_interface/demo_pose.cpp | 2 +- .../demos/cpp_interface/demo_twist.cpp | 2 +- .../include/moveit_servo/servo.hpp | 3 +- moveit_ros/moveit_servo/src/servo.cpp | 30 ++++++++++++------- moveit_ros/moveit_servo/src/servo_node.cpp | 10 ++++--- 6 files changed, 30 insertions(+), 19 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 bfac5ae194..e27b2256d0 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/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 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 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 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); } diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index deb8b02d01..9eae434869 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,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; }