Use angle distance function from pose util

AscendNTNU · Jul 25, 2019 · ce11e39 · ce11e39
1 parent 1ad0758
commit ce11e39
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 25 deletions.
diff --git a/src/states/move_state.cpp b/src/states/move_state.cpp
@@ -17,18 +17,7 @@ bool fluid::MoveState::hasFinishedExecution() {
     	   					std::abs(getCurrentTwist().twist.linear.y) < fluid::Core::velocity_completion_threshold && 
     	   					std::abs(getCurrentTwist().twist.linear.z) < fluid::Core::velocity_completion_threshold;
 
-    tf2::Quaternion quat(getCurrentPose().pose.orientation.x, 
-                         getCurrentPose().pose.orientation.y, 
-                         getCurrentPose().pose.orientation.z, 
-                         getCurrentPose().pose.orientation.w);
-
-    double roll, pitch, yaw;
-    tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
-    // If the quaternion is invalid, e.g. (0, 0, 0, 0), getRPY will return nan, so in that case we just set 
-    // it to zero. 
-    yaw = std::isnan(yaw) ? 0.0 : yaw;
-
-    bool atYawTarget = std::abs(setpoint.yaw - yaw) < fluid::Core::yaw_completion_threshold; 
+    bool atYawTarget = std::abs(PoseUtil::angleBetween(current_pose_.pose.orientation, setpoint.yaw)) < fluid::Core::yaw_completion_threshold; 
 
     return atYawTarget && atPositionTarget;
 }

diff --git a/src/states/rotate_state.cpp b/src/states/rotate_state.cpp
@@ -17,19 +17,9 @@ bool fluid::RotateState::hasFinishedExecution() {
     	   					std::abs(getCurrentTwist().twist.linear.y) < fluid::Core::velocity_completion_threshold && 
     	   					std::abs(getCurrentTwist().twist.linear.z) < fluid::Core::velocity_completion_threshold;
 
-    tf2::Quaternion quat(getCurrentPose().pose.orientation.x, 
-                         getCurrentPose().pose.orientation.y, 
-                         getCurrentPose().pose.orientation.z, 
-                         getCurrentPose().pose.orientation.w);
-
-    double roll, pitch, yaw;
-    tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
-    // If the quaternion is invalid, e.g. (0, 0, 0, 0), getRPY will return nan, so in that case we just set 
-    // it to zero. 
-    yaw = std::isnan(yaw) ? 0.0 : yaw;
-    const auto yaw_error = setpoint.yaw - yaw;
-
-    bool atYawTarget = std::abs(std::atan2(std::sin(yaw_error), std::cos(yaw_error))) < fluid::Core::yaw_completion_threshold; 
+
+
+    bool atYawTarget = std::abs(PoseUtil::angleBetween(current_pose_.pose.orientation, setpoint.yaw)) < fluid::Core::yaw_completion_threshold; 
 
     return atYawTarget && atPositionTarget;
 }