def test_quaternion_distance(self):
q1 = rpy2quaternion([0, 0, 0])
q2 = rpy2quaternion([0, 0, 0])
self.assertEqual(quaternion_distance(q1, q2), 0.0)
q1 = rpy2quaternion([np.pi, 0, 0])
q2 = rpy2quaternion([0, 0, 0])
self.assertEqual(quaternion_distance(q1, q2), np.pi)
self.assertEqual(quaternion_distance(np.ones(4), np.ones(4)), 0.0)
def rotation(self, rotation):
"""Set rotation of this coordinate
This setter checkes the given rotation and set it this coordinate.
Parameters
----------
rotation : list or numpy.ndarray
we can take 3x3 rotation matrix or
rpy angle [yaw, pitch, roll] or
quaternion [w, x, y, z] order
"""
rotation = np.array(rotation)
# Convert quaternions
if rotation.shape == (4, ):
self._q = np.array([q for q in rotation])
if np.abs(np.linalg.norm(self._q) - 1.0) > 1e-3:
raise ValueError('Invalid quaternion. Must be '
'norm 1.0, get {}'.format(
np.linalg.norm(self._q)))
rotation = quaternion2matrix(self._q)
elif rotation.shape == (3, ):
# Convert [yaw-pitch-roll] to rotation matrix
self._q = rpy2quaternion(rotation)
rotation = quaternion2matrix(self._q)
else:
self._q = matrix2quaternion(rotation)
# Convert lists and tuples
if type(rotation) in (list, tuple):
rotation = np.array(rotation).astype(np.float32)
_check_valid_rotation(rotation)
self._rotation = rotation * 1.
def dynamic_reconfigure_callback(self, config, level):
self.x = config['x']
self.y = config['y']
self.z = config['z']
self.roll = np.deg2rad(config['roll'])
self.pitch = np.deg2rad(config['pitch'])
self.yaw = np.deg2rad(config['yaw'])
self.q = rpy2quaternion([self.yaw, self.pitch, self.roll])
return config
def dynamic_reconfigure_callback(self, config, level):
self.x = config["x"]
self.y = config["y"]
self.z = config["z"]
self.w = config["w"]
self.d = config["d"]
self.h = config["h"]
self.roll = np.deg2rad(config['roll'])
self.pitch = np.deg2rad(config['pitch'])
self.yaw = np.deg2rad(config['yaw'])
self.q = rpy2quaternion([self.yaw, self.pitch, self.roll])
return config
def __init__(self):
self.pub = rospy.Publisher('~output', PoseStamped, queue_size=1)
self.pose_stamped = PoseStamped()
self.subscribe()
self.srv = Server(RelativePoseParamsConfig,
self.dynamic_reconfigure_callback)
self.x = rospy.get_param('~x', 0.)
self.y = rospy.get_param('~y', 0.)
self.z = rospy.get_param('~z', 0.)
self.roll = np.deg2rad(rospy.get_param('~roll', 0.))
self.pitch = np.deg2rad(rospy.get_param('~pitch', 0.))
self.yaw = np.deg2rad(rospy.get_param('~yaw', 0.))
self.q = rpy2quaternion([self.yaw, self.pitch, self.roll])
def __init__(self):
self.pub = rospy.Publisher("~output", BoundingBox, queue_size=1)
self.box = BoundingBox()
self.frame = rospy.get_param("~frame", "/base_link")
self.box.header.frame_id = self.frame
self.box.header.seq = 0
self.srv = Server(BoxParamsConfig, self.dynamic_reconfigure_callback)
self.x = rospy.get_param("~x", 0.)
self.y = rospy.get_param("~y", 0.)
self.z = rospy.get_param("~z", 0.)
self.w = rospy.get_param("~w", 0.2)
self.d = rospy.get_param("~d", 0.2)
self.h = rospy.get_param("~h", 0.2)
self.roll = np.deg2rad(rospy.get_param('~roll', 0.))
self.pitch = np.deg2rad(rospy.get_param('~pitch', 0.))
self.yaw = np.deg2rad(rospy.get_param('~yaw', 0.))
self.q = rpy2quaternion([self.yaw, self.pitch, self.roll])
self.timer = rospy.Timer(rospy.Duration(0.01), self.timer_callback)