Merge pull request #995 from tue-robotics/deduplicate_wrap_angle_pi

Import wrap_angle_pi from util.geometry helpers

challenge_dishwasher/src/challenge_dishwasher/open_dishwasher.py

@@ -9,6 +9,7 @@
 from robot_skills.util.kdl_conversions import frame_stamped, VectorStamped
 from robot_smach_states import NavigateToSymbolic
 from robot_smach_states.util.designators import EdEntityDesignator
+from robot_smach_states.util.geometry_helpers import wrap_angle_pi
 from smach import StateMachine, State, cb_interface, CBState
 from tf.transformations import euler_from_quaternion, quaternion_from_euler
 
@@ -31,20 +32,6 @@ def _get_yaw_from_quaternion_msg(msg):
     return yaw
 
 
-def _wrap_angle_pi(angle):
-    """
-    Wraps between -pi and +pi
-
-    :param angle: Input angle
-    :return: Wrapped angle
-    """
-    angle = angle % (2 * math.pi)
-    if angle > math.pi:
-        return angle - 2 * math.pi
-    elif angle < -math.pi:
-        return angle + 2 * math.pi
-    return angle
-
 
 ControlParameters = namedtuple('ControlParameters', [
     'position_gain',
@@ -101,7 +88,7 @@ def _get_target_delta_in_robot_frame(self, goal_pose):
         goal_pose.header.stamp = rospy.Time.now()
         pose = self._tf_buffer.transform(goal_pose, self.robot.robot_name + '/base_link', rospy.Duration(1.0))
         yaw = _get_yaw_from_quaternion_msg(pose.pose.orientation)
-        return pose.pose.position.x, pose.pose.position.y, _wrap_angle_pi(yaw)
+        return pose.pose.position.x, pose.pose.position.y, wrap_angle_pi(yaw)
 
     def _goal_reached(self, dx, dy, dyaw):
         return math.hypot(dx, dy) < self.params.goal_position_tolerance and abs(dyaw) < self.params.goal_rotation_tolerance

challenge_storing_groceries/src/challenge_storing_groceries/open_door.py

@@ -20,6 +20,7 @@
 import robot_smach_states as states
 
 import robot_smach_states.util.designators as ds
+from robot_smach_states.util.geometry_helpers import wrap_angle_pi
 
 
 def _clamp(abs_value, value):
@@ -45,21 +46,6 @@ def _get_yaw_from_quaternion_msg(msg):
     return yaw
 
 
-def _wrap_angle_pi(angle):
-    """
-    Wraps between -pi and +pi
-    -pi is excluded, (-pi, pi]
-
-    :param angle: Input angle
-    :return: Wrapped angle
-    """
-    angle = angle % (2 * math.pi)
-    if angle > math.pi:
-        return angle - 2 * math.pi
-    elif angle <= -math.pi:
-        return angle + 2 * math.pi
-    return angle
-
 
 class UpdateCabinetPose(smach.State):
     def __init__(self, robot, cabinet, cabinet_inspect_area):
@@ -99,7 +85,7 @@ def _get_target_delta_in_robot_frame(self, goal_pose):
         goal_pose.header.stamp = rospy.Time.now()
         pose = self._tf_buffer.transform(goal_pose, self.robot.robot_name + '/base_link', rospy.Duration(1.0))
         yaw = _get_yaw_from_quaternion_msg(pose.pose.orientation)
-        return pose.pose.position.x, pose.pose.position.y, _wrap_angle_pi(yaw)
+        return pose.pose.position.x, pose.pose.position.y, wrap_angle_pi(yaw)
 
     def _goal_reached(self, dx, dy, dyaw):
         return math.hypot(dx, dy) < self._goal_position_tolerance and abs(dyaw) < self._goal_rotation_tolerance