How do I remove rotation from a pose? [closed]
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?
If you're not interested in the orientation of a pose, why not overwrite it with
Quaternion(0, 0, 0, 1)
?This would not give a solution, as the xyz coordinates of the pose are influenced by the orientation itself. It is not so much that I am not interested in the orientation, but rather that I am interested in an orientation of Quaternion(0, 0, 0, 1). However, setting the orientation to this artificially does not solve the issue.
Anyway, I solved the issue and it was from dirty data at the beginning of the pipeline, not from the above code. The above code appears to work.