Hi all,

I am using ROS Melodic in Gazebo 9 with Ardupilot to simulate the standard Iris quadocopter. The drone carries a camera on a gimbal which is used to detect a WhyCon fiducial marker. I am carrying out coordinate frame transforms using the tf2 library and I believe that my operations are correct so far, except that the final determined pose of the marker relative to the drone has non-zero rotation (as in w,x,y,z != 1,0,0,0). The values for the pose seem correct generally, but in order to use them I need to have "unrotated" values, as I am using these values to direct the drone toward the marker. I am attempting to create a transform in order to zero-out the rotation and give me the corresponding point in space:

geometry_msgs::PoseStamped straighten_pose( const geometry_msgs::PoseStamped & _pose_in )
{
    // declare pose variables
    geometry_msgs::PoseStamped pose_out;
    geometry_msgs::PoseStamped pose_in = _pose_in;

    // make the child_frame_id equal to the parent's frame_id with '_straightened' appended
    std::string child_frame_id = pose_in.header.frame_id + "_straightened";

    // access the transform broadcaster
    static tf2_ros::TransformBroadcaster transform_broadcaster;

    // declare a message in which to store the new transform
    geometry_msgs::TransformStamped transform_stamped_message;

    // set the transform's header
    transform_stamped_message.header.stamp = ros::Time::now();
    transform_stamped_message.header.frame_id = pose_in.header.frame_id;
    transform_stamped_message.child_frame_id = child_frame_id;

    // set the rotation from the parent to the child
    transform_stamped_message.transform.rotation = pose_in.pose.orientation;

    // send the transform
    transform_broadcaster.sendTransform(transform_stamped_message);

    // transform the pose and return it
    return transform_buffer.transform(pose_in, child_frame_id, ros::Duration(0.05));
}

I believe this works in theory, but I could be mistaken. In practice it does reduce the rotation to approximately (w,x,y,z)=(1,0,0,0). I say "approximately" because the 0s are more like some tiny number ~ 1e-50, and the w is similarly close to 1. Most of the time, this gives me the values that I am looking for, but sometimes there are non-negligible jumps in the pose_out.position which makes the code practically unusable. These jumps are associated with similarly unpredictable changes in the signs of the "zero" components in the pose's orientation. Can anyone suggest a better way of doing this, which does not incur the (probable) floating point errors that I am experiencing now? Or even better, could someone suggest a way that has already been implemented and is reliable?