Exemplo n.º 1
0
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 << ").");
  }
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
0
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 << ").");
  }
}
Exemplo n.º 4
0
/* 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();
  }
}
Exemplo n.º 5
0
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);
    }
}