Lookup would require extrapolation into the past. Requested time 7.175000000 but the earliest data is at time 7.334000000, when looking up transform from frame [camera_rgb_optical_frame] to frame [world].
Hey. I am trying to transforms point cloud from /camera_rgb_optical_frame
frame to /world
frame but getting below error:
[ERROR] [1593113184.201010787, 12.387000000]: Lookup would require extrapolation into the past. Requested time 7.175000000 but the earliest data is at time 7.334000000, when looking up transform from frame [camera_rgb_optical_frame] to frame [world]
Many people have faced this problem before and there are also solutions given for this, but unfortunately it's not working in my case. I recently started working on ros transform.
Output of rosrun tf view_frames
:
Code:
class CloudTransformer
{
public:
explicit CloudTransformer(ros::NodeHandle nh)
: nh_(nh)
{
// Define Publishers and Subscribers here
pcl_sub_ = nh_.subscribe("/camera/depth_registered/points", 1, &CloudTransformer::pclCallback, this);
pcl_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/transformed_point_cloud", 1);
buffer_.reset(new sensor_msgs::PointCloud2);
buffer_->header.frame_id = "/world";
}
private:
ros::NodeHandle nh_;
ros::Subscriber pcl_sub_;
ros::Publisher pcl_pub_;
tf::TransformListener listener_;
sensor_msgs::PointCloud2::Ptr buffer_;
void pclCallback(const sensor_msgs::PointCloud2ConstPtr& pcl_msg)
{
try{
listener_.waitForTransform("/world", (*pcl_msg).header.frame_id, (*pcl_msg).header.stamp, ros::Duration(5.0));
//listener_.waitForTransform("/world", "camera_rgb_optical_frame", ros::Time::now(), ros::Duration(5.0));
}catch(tf::TransformException &ex){
ROS_WARN("%s",ex.what());
};
pcl_ros::transformPointCloud("/world", *pcl_msg, *buffer_, listener_);
pcl_pub_.publish(buffer_);
}
};
A little help or guidance would be much appreciated. Thanks in advance.
EDIT:
Output of rosrun tf tf_monitor world camera_rgb_optical_frame
Waiting for transform chain to become available between world and camera_rgb_optical_frame
RESULTS: for world to camera_rgb_optical_frame
Chain is: world -> camera_rgb_optical_frame
Net delay avg = -0.0099055: max = 0.003
Frames:
Frame: camera_rgb_optical_frame published by unknown_publisher Average Delay: -0.1 Max Delay: 0
Frame: world published by unknown_publisher Average Delay: -0.1 Max Delay: 0
All Broadcasters:
Node: unknown_publisher 71.7489 Hz, Average Delay: -0.0281875 Max Delay: 0.016
Node: unknown_publisher(static) 1e+08 Hz, Average Delay: 0 Max Delay: 0
RESULTS: for world to camera_rgb_optical_frame
Chain is: world -> camera_rgb_optical_frame
Net delay avg = -0.0181717: max = 0.003
Frames:
Frame: camera_rgb_optical_frame published by unknown_publisher Average Delay: -0.09975 Max Delay: 0
Frame: world published by unknown_publisher Average Delay: -0.1 Max Delay: 0
All Broadcasters:
Node: unknown_publisher 70.4225 Hz, Average Delay: -0.0244 Max Delay: 0.02
Node: unknown_publisher(static) 1e+08 Hz, Average Delay: 0 Max Delay: 0
RESULTS: for world to camera_rgb_optical_frame
Chain is: world -> camera_rgb_optical_frame
Net delay avg = -0.0239486: max = 0.003
Frames:
Frame: camera_rgb_optical_frame published by unknown_publisher Average Delay: -0.0998333 Max Delay: 0
Frame: world published by unknown_publisher Average Delay: -0.1 Max Delay: 0
All Broadcasters:
Node: unknown_publisher 69.2771 Hz, Average Delay: -0.0231087 Max Delay: 0.024
Node: unknown_publisher(static) 1e+08 Hz, Average Delay: 0 Max Delay: 0
RESULTS: for world to camera_rgb_optical_frame
Chain is: world -> camera_rgb_optical_frame
Net delay avg = -0.0291794: max = 0.003
Frames:
Frame: camera_rgb_optical_frame published by unknown_publisher Average Delay: -0.0998889 Max Delay: 0
Frame: world published by unknown_publisher Average Delay: -0.1 Max Delay: 0
All Broadcasters:
Node: unknown_publisher 71.5859 Hz, Average Delay: -0.0246308 Max Delay: 0.024
Node: unknown_publisher(static) 1e+08 Hz, Average ...