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;
}
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;
}