Esempio n. 1
0
  bool transformVectorTo(const tf::Transformer& tf, const string& source_frame, const string& goal_frame, const Time& time_source,
                         const geometry_msgs::Vector3& point_in, geometry_msgs::Vector3& point_out, const std::string& fixed_frame, const Time& time_goal)
  {
    ros::Duration timeout = Duration().fromSec(2.0);
    if (!tf.waitForTransform(source_frame, time_source, goal_frame, time_goal, fixed_frame, timeout)) return false;
    tf::Stamped<tf::Point> pnt(tf::Vector3(point_in.x, point_in.y, point_in.z), time_source, source_frame);
    tf.transformVector(goal_frame, time_goal, pnt, fixed_frame, pnt);
    point_out.x = pnt[0];
    point_out.y = pnt[1];
    point_out.z = pnt[2];

    return true;
  }
Esempio n. 2
0
bool projectRayToGround(const tf::Transformer& listener,
      const tf::Stamped<tf::Point> camera_ray,
      Eigen::Vector4d ground_plane, std::string ground_frame,
      tf::Stamped<tf::Point>* world_point)
{
    tf::StampedTransform camera_transform;
    // This is a static link, so Time(0) should be fine
    if (!listener.canTransform(ground_frame, camera_ray.frame_id_, camera_ray.stamp_)) {
        ROS_INFO("Couldn't transform %s to %s\n",camera_ray.frame_id_.c_str(),
                ground_frame.c_str());
        return false;
    }
    listener.lookupTransform(ground_frame, camera_ray.frame_id_,ros::Time(0),camera_transform);
    tf::Stamped<tf::Point> ground_frame_ray;
    listener.transformVector(ground_frame, camera_ray, ground_frame_ray);
    Eigen::Vector3d ray, ray_origin;
    tf::vectorTFToEigen(ground_frame_ray, ray);
    tf::vectorTFToEigen(camera_transform.getOrigin(), ray_origin);
    Eigen::Vector3d ground_v = intersectRayPlane(ray, ray_origin, ground_plane); 
    tf::vectorEigenToTF(ground_v, *world_point);
    world_point->frame_id_ = ground_frame;
    world_point->stamp_ = camera_ray.stamp_;
    return true;
}