Fiducials in conjuction with AMCL
I'm having some trouble getting fiducials (using aruco_detect) to work alongside amcl. The goal here is to use normal amcl localization when a fiducial is not present, and to switch to localization using fiducials when the camera sees a fiducial. After having a lot of trouble trying to use fiducial_slam for this, I decided to create my own node that subscribes to /fiducial_transforms (published by aruco_detect) and publishes to /initialpose (subscribed to by amcl).
While the subscribing and publishing works well with both aruco and amcl, I have been running into problems with the trigonometry involved in obtaining the robot's position and orientation. Since the robots position is relative to a 2D map (already created), I converted the rotation quaternions into euler angles and only considered the yaw value (using tf.transformations.euler_from_quaternion). To reconstruct the quaternion for publishing, I use tf.transformations.quaternion_from_euler, passing in (0, 0, new_yaw). After playing around with my calculation of new_yaw (including passing in -new_yaw, pi-new_yaw, pi/2-new_yaw, etc.), I was unable to un-invert the robot's rotation (a rotation of 30 degrees of the robot always corresponds to a rotation of negative 30 degrees in the quaternion).
My current assessment of the problem is that there is some disconnect between the axes of the 2D map and the response from aruco_detect, which for some reason may cause the rotation to always happen in a certain direction (though this does not explain why publishing the quaternions for (0, 0, new_yaw) and (0, 0, -new_yaw) have the same outcome, so I am still unsure). In addition, even though we are assuming for now that the fiducial is at the origin (facing in the quaternion direction (0, 0, 1, 0)), we would like to incorporate known values for the fiducial's position and rotation to obtain the robot's true pose.
