bool TabletopSegmentor::getPlanePoints (const pcl::PointCloud<PointT> &table,
const tf::Transform& table_plane_trans,
sensor_msgs::PointCloud &table_points)
{
// Prepare the output
table_points.header = table.header;
table_points.points.resize (table.points.size ());
for (size_t i = 0; i < table.points.size (); ++i)
{
table_points.points[i].x = table.points[i].x;
table_points.points[i].y = table.points[i].y;
table_points.points[i].z = table.points[i].z;
}
// Transform the data
tf::TransformListener listener;
tf::StampedTransform table_pose_frame(table_plane_trans, table.header.stamp,
table.header.frame_id, "table_frame");
listener.setTransform(table_pose_frame);
std::string error_msg;
if (!listener.canTransform("table_frame", table_points.header.frame_id, table_points.header.stamp, &error_msg))
{
ROS_ERROR("Can not transform point cloud from frame %s to table frame; error %s",
table_points.header.frame_id.c_str(), error_msg.c_str());
return false;
}
int current_try=0, max_tries = 3;
while (1)
{
bool transform_success = true;
try
{
listener.transformPointCloud("table_frame", table_points, table_points);
}
catch (tf::TransformException ex)
{
transform_success = false;
if ( ++current_try >= max_tries )
{
ROS_ERROR("Failed to transform point cloud from frame %s into table_frame; error %s",
table_points.header.frame_id.c_str(), ex.what());
return false;
}
//sleep a bit to give the listener a chance to get a new transform
ros::Duration(0.1).sleep();
}
if (transform_success) break;
}
table_points.header.stamp = table.header.stamp;
table_points.header.frame_id = "table_frame";
return true;
}
bool TableDetector::getPlanePoints (const pcl::PointCloud<Point> &table,
const tf::Transform& table_plane_trans,
sensor_msgs::PointCloud &table_points)
{
// Prepare the output
table_points.header.frame_id = table.header.frame_id;
table_points.header.stamp = table.header.stamp;
table_points.points.resize (table.points.size ());
for (size_t i = 0; i < table.points.size (); ++i)
{
table_points.points[i].x = table.points[i].x;
table_points.points[i].y = table.points[i].y;
table_points.points[i].z = table.points[i].z;
}
// Transform the data
tf::TransformListener listener;
tf::StampedTransform table_pose_frame(table_plane_trans, table.header.stamp,
table.header.frame_id, "table_frame");
listener.setTransform(table_pose_frame);
std::string error_msg;
if (!listener.canTransform("table_frame", table_points.header.frame_id, table_points.header.stamp, &error_msg))
{
ROS_ERROR("Can not transform point cloud from frame %s to table frame; error %s",
table_points.header.frame_id.c_str(), error_msg.c_str());
return false;
}
try
{
listener.transformPointCloud("table_frame", table_points, table_points);
}
catch (tf::TransformException ex)
{
ROS_ERROR("Failed to transform point cloud from frame %s into table_frame; error %s",
table_points.header.frame_id.c_str(), ex.what());
return false;
}
table_points.header.stamp = ros::Time::now();
table_points.header.frame_id = "table_frame";
return true;
}
