[planning][hand] refactor the if-else logic based on the rule of "ret…

…urn as early as possible"

aerial_robot_control/src/nmpc/tilt_mt_servo_dist_nmpc_controller.cpp

@@ -101,12 +101,21 @@ void nmpc::TiltMtServoDistNMPC::updateITerm()
 
   /* update I term */
   tf::Vector3 vel = estimator_->getVel(Frame::COG, estimate_mode_);
-  // if the norm of vel is less than 0.2 m/s, than the I term is updated.
-  if (vel.length() < 0.1)
-  {
-    wrench_est_i_term_.update(target_pos, target_q, pos, q);
-  }
+  // if the norm of vel is larger than a threshold, the I term should be updated.
+  if (vel.length() >= 0.1)
+    return;
+
+  auto external_force_w = wrench_est_ptr_->getDistForceW();
+  auto external_torque_cog = wrench_est_ptr_->getDistTorqueCOG();
+
+  if (external_force_w.x > 0.5)  //|| mpc_solver_ptr_.
+    target_pos.setX(pos.getX());
+  if (external_force_w.y > 0.5)
+    target_pos.setY(pos.getY());
+  if (external_force_w.z > 0.5)
+    target_pos.setZ(pos.getZ());
 
+  wrench_est_i_term_.update(target_pos, target_q, pos, q);
 }
 
 void nmpc::TiltMtServoDistNMPC::prepareNMPCParams()

aerial_robot_planning/scripts/mpc_smach_node.py

@@ -285,7 +285,6 @@ def __init__(self):
         self.height_threshold = 0.05
         self.direction_hold_time = 1
         self.rate = rospy.Rate(20)
-        self.is_finished = False
 
     def execute(self, userdata):
 
@@ -326,19 +325,24 @@ def execute(self, userdata):
             is_z_angular_alignment = abs(euler_angles[2]) < self.z_angle_threshold
             is_z_height_alignment = abs(z_difference) < self.height_threshold
 
-            if is_x_angular_alignment and is_y_angular_alignment and is_z_angular_alignment and is_z_height_alignment:
+            is_in_thresh = (is_x_angular_alignment and is_y_angular_alignment and is_z_angular_alignment and is_z_height_alignment)
+
+            if not is_in_thresh:
+                self.last_threshold_time = None
+
+            if is_in_thresh:
                 if self.last_threshold_time is None:
                     self.last_threshold_time = rospy.get_time()
-                elif rospy.get_time() - self.last_threshold_time > self.direction_hold_time:
-                    self.is_finished = True
-            else:
-                self.last_threshold_time = None
-            if self.is_finished:
-                rospy.loginfo("")
-                rospy.loginfo("Current state: Unlock")
-                return "go_unlock"
+
+                if rospy.get_time() - self.last_threshold_time > self.direction_hold_time:
+                    break
+
             self.rate.sleep()
 
+        rospy.loginfo("")
+        rospy.loginfo("Current state: Unlock")
+        return "go_unlock"
+
 
 class UnlockState(smach.State):
     def __init__(self):