def test_x_axis_diagonal_rotation(self):
orientation_pose = Pose()
orientation_pose.position.x = 0.0
orientation_pose.position.y = 0.0
orientation_pose.position.z = 0.0
orientation_pose.orientation.x = 0.712590112587
orientation_pose.orientation.y = -0.00994445446764
orientation_pose.orientation.z = 0.701496927312
orientation_pose.orientation.w = -0.00430119065513
current_pose = Pose()
current_pose.position.x = 0.0
current_pose.position.y = 0.0
current_pose.position.z = 0.0
current_pose.orientation.x = 0.888835870709
current_pose.orientation.y = -0.277907278582
current_pose.orientation.z = 0.349356863595
current_pose.orientation.w = 0.103383370571
threshold_angle = 50
result = orientation(orientation_pose,
current_pose,
threshold_angle,
axis="x")
self.assertEquals(result, 1)
threshold_angle = 40
result = orientation(orientation_pose,
current_pose,
threshold_angle,
axis="x")
self.assertEquals(result, 0)
def test_x_axis_rotation_around_z(self):
orientation_pose = Pose()
orientation_pose.position.x = 0.0
orientation_pose.position.y = 0.0
orientation_pose.position.z = 0.0
orientation_pose.orientation.x = 0.712590112587
orientation_pose.orientation.y = -0.00994445446764
orientation_pose.orientation.z = 0.701496927312
orientation_pose.orientation.w = -0.00430119065513
current_pose = Pose()
current_pose.position.x = 0.0
current_pose.position.y = 0.0
current_pose.position.z = 0.0
current_pose.orientation.x = -0.497165522925
current_pose.orientation.y = 0.508076415174
current_pose.orientation.z = -0.499403654355
current_pose.orientation.w = 0.495258305509
threshold_angle = 95
result = orientation(orientation_pose,
current_pose,
threshold_angle,
axis="x")
self.assertEquals(result, 1)
threshold_angle = 85
result = orientation(orientation_pose,
current_pose,
threshold_angle,
axis="x")
self.assertEquals(result, 0)
def test_angled_orientation_with_roll_pitch_yaw(self):
# R: 20, P: 10, Y: 10
orientation_pose = Pose()
orientation_pose.position.x = 0.0
orientation_pose.position.y = 0.0
orientation_pose.position.z = 0.0
orientation_pose.orientation.x = -0.07043066500709379
orientation_pose.orientation.y = -0.10057509181462758
orientation_pose.orientation.z = 0.16484914083814878
orientation_pose.orientation.w = 0.9786464801439086
# R: 20, P: 20, Y: 20
current_pose = Pose()
current_pose.position.x = 0.0
current_pose.position.y = 0.0
current_pose.position.z = 0.0
current_pose.orientation.x = -0.13870300568743923
current_pose.orientation.y = -0.19810996120146637
current_pose.orientation.z = -0.13870300568743923
current_pose.orientation.w = 0.9603517041684749
threshold_angle = 10
result = orientation(orientation_pose, current_pose, threshold_angle)
self.assertEquals(result, 0)
def test_angled_orientation_no_rotation(self):
# R: 0, P: 20, Y: 0
orientation_pose = Pose()
orientation_pose.position.x = 0.0
orientation_pose.position.y = 0.0
orientation_pose.position.z = 0.0
orientation_pose.orientation.x = 0.0
orientation_pose.orientation.y = 0.1736709
orientation_pose.orientation.z = 0.0
orientation_pose.orientation.w = 0.9848037
# R: 0, P: 45, Y: 0
current_pose = Pose()
current_pose.position.x = 0.0
current_pose.position.y = 0.0
current_pose.position.z = 0.0
current_pose.orientation.x = 0.0
current_pose.orientation.y = -0.38270469415797664
current_pose.orientation.z = 0.0
current_pose.orientation.w = 0.9238707253016787
threshold_angle = 20
result = orientation(orientation_pose, current_pose, threshold_angle)
self.assertEquals(result, 0)
def test_angled_orientation_pitch_only(self):
# R: 0, P: 20, Y: 0
orientation_pose = Pose()
orientation_pose.position.x = 0.0
orientation_pose.position.y = 0.0
orientation_pose.position.z = 0.0
orientation_pose.orientation.x = 0.0
orientation_pose.orientation.y = -0.17367092187156372
orientation_pose.orientation.z = 0.0
orientation_pose.orientation.w = 0.9848037423244701
# R: 0, P: 45, Y: 0
current_pose = Pose()
current_pose.position.x = 0.0
current_pose.position.y = 0.0
current_pose.position.z = 0.0
current_pose.orientation.x = 0.0
current_pose.orientation.y = -0.38270469415797664
current_pose.orientation.z = 0.0
current_pose.orientation.w = 0.9238707253016787
threshold_angle = 26
result = orientation(orientation_pose, current_pose, threshold_angle)
self.assertEquals(result, 1)
def test_upside_down(self):
orientation_pose = Pose()
orientation_pose.position.x = 0.0
orientation_pose.position.y = 0.0
orientation_pose.position.z = 0.0
orientation_pose.orientation.x = 0.0
orientation_pose.orientation.y = 0.0
orientation_pose.orientation.z = 0.0
orientation_pose.orientation.w = 1.0
# R: 0, P: 175, Y: 0
current_pose = Pose()
current_pose.position.x = 0.0
current_pose.position.y = 0.0
current_pose.position.z = 0.0
current_pose.orientation.x = 0.0
current_pose.orientation.y = 0.9990483518732844
current_pose.orientation.z = 0.0
current_pose.orientation.w = -0.043616403098766035
threshold_angle = 25
result = orientation(orientation_pose, current_pose, threshold_angle)
self.assertEquals(result, 0)
def test_yaw_has_no_effect(self):
orientation_pose = Pose()
orientation_pose.position.x = 0.0
orientation_pose.position.y = 0.0
orientation_pose.position.z = 0.0
orientation_pose.orientation.x = 0.0
orientation_pose.orientation.y = 0.0
orientation_pose.orientation.z = 0.0
orientation_pose.orientation.w = 1.0
# R: 0, P: 0, Y: 20
current_pose = Pose()
current_pose.position.x = 0.0
current_pose.position.y = 0.0
current_pose.position.z = 0.0
current_pose.orientation.x = 0.0
current_pose.orientation.y = 0.0
current_pose.orientation.z = -0.17367092187156372
current_pose.orientation.w = 0.9848037423244701
threshold_angle = 5
result = orientation(orientation_pose, current_pose, threshold_angle)
self.assertEquals(result, 1)
def test_not_orientation_pitch_and_roll(self):
orientation_pose = Pose()
orientation_pose.position.x = 0.0
orientation_pose.position.y = 0.0
orientation_pose.position.z = 0.0
orientation_pose.orientation.x = 0.0
orientation_pose.orientation.y = 0.0
orientation_pose.orientation.z = 0.0
orientation_pose.orientation.w = 1.0
# R: 35, P:-15, Y: 0
current_pose = Pose()
current_pose.position.x = 0.0
current_pose.position.y = 0.0
current_pose.position.z = 0.0
current_pose.orientation.x = -0.2981390886391489
current_pose.orientation.y = 0.12450664849252854
current_pose.orientation.z = -0.03926361711858248
current_pose.orientation.w = 0.9455525086833311
threshold_angle = 25
result = orientation(orientation_pose, current_pose, threshold_angle)
self.assertEquals(result, 0)
def test_orientation_pitch_and_roll(self):
orientation_pose = Pose()
orientation_pose.position.x = 0.0
orientation_pose.position.y = 0.0
orientation_pose.position.z = 0.0
orientation_pose.orientation.x = 0.0
orientation_pose.orientation.y = 0.0
orientation_pose.orientation.z = 0.0
orientation_pose.orientation.w = 1.0
# R: 10, P: 10, Y: 0
current_pose = Pose()
current_pose.position.x = 0.0
current_pose.position.y = 0.0
current_pose.position.z = 0.0
current_pose.orientation.x = -0.08683579867169378
current_pose.orientation.y = -0.0868357986716937
current_pose.orientation.z = 0.007582702465964556
current_pose.orientation.w = 0.9924019300472017
threshold_angle = 25
result = orientation(orientation_pose, current_pose, threshold_angle)
self.assertEquals(result, 1)