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

moveit · Aug 7, 2024 · 9975f8b · 9975f8b
1 parent f30d51b
commit 9975f8b
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 10 deletions.
diff --git 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;
 }
@@ -665,9 +661,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 +674,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);
 }
 

diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp
@@ -303,6 +303,8 @@ void ServoNode::servoLoop()
   }
   KinematicState current_state = servo_->getCurrentRobotState();
   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();