void tf2::lookupTransform(const tf2_ros::Buffer& tf_buffer,
const std::string& targetFrame,
const std::string& sourceFrame,
const ros::Time& time,
tf2::Transform& sourceToTarget)
{
// First try to transform the data at the requested time
try
{
tf2::fromMsg(tf_buffer.lookupTransform(targetFrame, sourceFrame, time).transform, sourceToTarget);
}
catch (tf2::TransformException& ex)
{
// The issue might be that the transforms that are available are not close
// enough temporally to be used. In that case, just use the latest available
// transform and warn the user.
// If this also fails, then just throw an exception.
geometry_msgs::TransformStamped transformStamped = tf_buffer.lookupTransform(targetFrame, sourceFrame, ros::Time(0));
tf2::fromMsg(transformStamped.transform, sourceToTarget);
ROS_WARN_STREAM_THROTTLE(2.0, "Transform from " << sourceFrame << " to " << targetFrame <<
" was unavailable for the time requested (" << time << "). " <<
"Using nearest at time " << transformStamped.header.stamp <<
" instead (" << transformStamped.header.stamp - time << ").");
}
}
bool tf2::lookupTransformSafe(const tf2_ros::Buffer& tf_buffer,
const std::string& targetFrame,
const std::string& sourceFrame,
const ros::Time& time,
tf2::Transform& sourceToTarget)
{
bool retVal = true;
// First try to transform the data at the requested time
try
{
tf2::fromMsg(tf_buffer.lookupTransform(targetFrame, sourceFrame, time).transform, sourceToTarget);
}
catch (tf2::TransformException& ex)
{
// The issue might be that the transforms that are available are not close
// enough temporally to be used. In that case, just use the latest available
// transform and warn the user.
try
{
geometry_msgs::TransformStamped transformStamped = tf_buffer.lookupTransform(targetFrame, sourceFrame, ros::Time(0));
tf2::fromMsg(transformStamped.transform, sourceToTarget);
ROS_WARN_STREAM_THROTTLE(2.0, "Transform from " << sourceFrame << " to " << targetFrame <<
" was unavailable for the time requested (" << time << "). " <<
"Using nearest at time " << transformStamped.header.stamp <<
" instead (" << transformStamped.header.stamp - time << ").");
}
catch(tf2::TransformException& ex)
{
ROS_WARN_STREAM_THROTTLE(2.0, "Could not obtain transform from " << sourceFrame <<
" to " << targetFrame << ". Error was " << ex.what() << "\n");
retVal = false;
}
}
// Transforming from a frame id to itself can fail when the tf tree isn't
// being broadcast (e.g., for some bag files). This is the only failure that
// would throw an exception, so check for this situation before giving up.
if (not retVal)
{
if (targetFrame == sourceFrame)
{
sourceToTarget.setIdentity();
retVal = true;
}
}
return retVal;
}
void tf2::waitForTransform(const tf2_ros::Buffer& tf_buffer,
const std::string& targetFrame,
const std::string& sourceFrame,
const ros::Time& time,
const ros::Duration& timeout,
tf2::Transform& sourceToTarget)
{
// First try to transform the data at the requested time
try
{
tf2::fromMsg(tf_buffer.lookupTransform(targetFrame, sourceFrame, time, timeout).transform, sourceToTarget);
}
catch(tf2::LookupException& ex)
{
ROS_WARN_STREAM_THROTTLE(2.0, "Frames " << sourceFrame << " or " << targetFrame <<
" doesn't currently exist, waiting and retrying..");
waitUntilCanTransform(tf_buffer, targetFrame, sourceFrame, time, timeout);
tf2::fromMsg(tf_buffer.lookupTransform(targetFrame, sourceFrame, time).transform, sourceToTarget);
}
catch(tf2::ConnectivityException& ex)
{
ROS_WARN_STREAM_THROTTLE(2.0, "Frames " << sourceFrame << " or " << targetFrame <<
" aren't connected on the tf tree, waiting and retrying..");
waitUntilCanTransform(tf_buffer, targetFrame, sourceFrame, time, timeout);
tf2::fromMsg(tf_buffer.lookupTransform(targetFrame, sourceFrame, time).transform, sourceToTarget);
}
catch (tf2::TransformException& ex)
{
// The issue might be that the transforms that are available are not close
// enough temporally to be used. In that case, just use the latest available
// transform and warn the user.
// If this also fails, then just throw an exception.
geometry_msgs::TransformStamped transformStamped = tf_buffer.lookupTransform(targetFrame, sourceFrame, ros::Time(0));
tf2::fromMsg(transformStamped.transform, sourceToTarget);
ROS_WARN_STREAM_THROTTLE(2.0, "Transform from " << sourceFrame << " to " << targetFrame <<
" was unavailable for the time requested (" << time << "). " <<
"Using nearest at time " << transformStamped.header.stamp <<
" instead (" << transformStamped.header.stamp - time << ").");
}
}
/* This method allows to wait for a transformation between frames that aren't on the current TF Tree.
* On simulated environment now() won't advance unless the clock is being publish.
* If there is no one publishing clock, then the waiting will persist */
void waitUntilCanTransform(const tf2_ros::Buffer& tf_buffer,
const std::string& targetFrame,
const std::string& sourceFrame,
const ros::Time& time,
const ros::Duration& timeout)
{
// poll for transform if timeout is set
ros::Time start_time = ros::Time::now();
while (ros::Time::now() < start_time + timeout &&
!tf_buffer.canTransform(targetFrame, sourceFrame, time) &&
(ros::Time::now()+ros::Duration(3.0) >= start_time) && //don't wait when we detect a bag loop
(ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf)
{
ros::Duration(0.01).sleep();
}
}
tuple<bool, geometry_msgs::TransformStamped>
maybe_get_transform(string const &parent_frame, string const &child_frame,
const tf2_ros::Buffer &buffer) {
bool got_transform = false;
geometry_msgs::TransformStamped current_transformation;
try {
current_transformation = buffer.lookupTransform(parent_frame,
child_frame,
ros::Time::now(),
ros::Duration(1.0));
got_transform = true;
}
catch (tf2::TransformException &ex) {
ROS_WARN("error: %s", ex.what());
}
return tuple<bool, geometry_msgs::TransformStamped>(got_transform,
current_transformation);
}
void FiducialsNode::location_cb(int id, double x, double y, double z,
double bearing) {
ROS_INFO("location_announce:id=%d x=%f y=%f bearing=%f",
id, x, y, bearing * 180. / 3.1415926);
visualization_msgs::Marker marker = createMarker(position_namespace, id);
ros::Time now = marker.header.stamp;
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.pose = scale_position(x, y, z, bearing);
marker.scale.x = 0.2 / scale;
marker.scale.y = 0.05 / scale;
marker.scale.z = 0.05 / scale;
marker.color = position_color;
marker.lifetime = ros::Duration();
marker_pub->publish(marker);
// TODO: subtract out odometry position, and publish transform from
// map to odom
double tf_x = marker.pose.position.x;
double tf_y = marker.pose.position.y;
double tf_yaw = bearing;
// publish a transform based on the position
if( use_odom ) {
// if we're using odometry, look up the odom transform and subtract it
// from our position so that we can publish a map->odom transform
// such that map->odom->base_link reports the correct position
std::string tf_err;
if( tf_buffer.canTransform(pose_frame, odom_frame, now,
ros::Duration(0.1), &tf_err ) ) {
// get odometry position from TF
tf2::Quaternion tf_quat;
tf_quat.setRPY(0.0, 0.0, tf_yaw);
tf2::Transform pose(tf_quat, tf2::Vector3(tf_x, tf_y, 0));
// look up camera transform if we can
if( last_camera_frame.length() > 0 ) {
if( tf_buffer.canTransform(pose_frame, last_camera_frame, now,
ros::Duration(0.1), &tf_err) ) {
geometry_msgs::TransformStamped camera_tf;
camera_tf = tf_buffer.lookupTransform(pose_frame,
last_camera_frame, now);
tf2::Transform camera = msg_to_tf(camera_tf);
pose = pose * camera.inverse();
} else {
ROS_ERROR("Cannot look up transform from %s to %s: %s",
pose_frame.c_str(), last_camera_frame.c_str(), tf_err.c_str());
}
}
geometry_msgs::TransformStamped odom;
odom = tf_buffer.lookupTransform(odom_frame, pose_frame, now);
tf2::Transform odom_tf = msg_to_tf(odom);
// M = C * O
// C^-1 * M = O
// C^-1 = O * M-1
tf2::Transform odom_correction = (odom_tf * pose.inverse()).inverse();
geometry_msgs::TransformStamped transform;
tf2::Vector3 odom_correction_v = odom_correction.getOrigin();
transform.transform.translation.x = odom_correction_v.getX();
transform.transform.translation.y = odom_correction_v.getY();
transform.transform.translation.z = odom_correction_v.getZ();
tf2::Quaternion odom_correction_q = odom_correction.getRotation();
transform.transform.rotation.x = odom_correction_q.getX();
transform.transform.rotation.y = odom_correction_q.getY();
transform.transform.rotation.z = odom_correction_q.getZ();
transform.transform.rotation.w = odom_correction_q.getW();
transform.header.stamp = now;
transform.header.frame_id = world_frame;
transform.child_frame_id = odom_frame;
//tf2::transformTF2ToMsg(odom_correction, transform, now, world_frame,
//odom_frame);
if (publish_tf)
tf_pub.sendTransform(transform);
} else {
ROS_ERROR("Can't look up base transform from %s to %s: %s",
pose_frame.c_str(),
odom_frame.c_str(),
tf_err.c_str());
}
} else {
// we're publishing absolute position
geometry_msgs::TransformStamped transform;
transform.header.stamp = now;
transform.header.frame_id = world_frame;
transform.child_frame_id = pose_frame;
transform.transform.translation.x = tf_x;
transform.transform.translation.y = tf_y;
transform.transform.translation.z = 0.0;
transform.transform.rotation = tf::createQuaternionMsgFromYaw(tf_yaw);
if (publish_tf)
tf_pub.sendTransform(transform);
}
}
