void LaserscanMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan, std::string topic)
{
sensor_msgs::PointCloud tmpCloud1,tmpCloud2;
sensor_msgs::PointCloud2 tmpCloud3;
// Verify that TF knows how to transform from the received scan to the destination scan frame
tfListener_.waitForTransform(scan->header.frame_id.c_str(), destination_frame.c_str(), scan->header.stamp, ros::Duration(1));
projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, tmpCloud1, tfListener_);
try
{
tfListener_.transformPointCloud(destination_frame.c_str(), tmpCloud1, tmpCloud2);
}catch (tf::TransformException ex){ROS_ERROR("%s",ex.what());return;}
for(int i=0; i<input_topics.size(); ++i)
{
if(topic.compare(input_topics[i]) == 0)
{
sensor_msgs::convertPointCloudToPointCloud2(tmpCloud2,tmpCloud3);
pcl_conversions::toPCL(tmpCloud3, clouds[i]);
clouds_modified[i] = true;
}
}
// Count how many scans we have
int totalClouds = 0;
for(int i=0; i<clouds_modified.size(); ++i)
if(clouds_modified[i])
++totalClouds;
// Go ahead only if all subscribed scans have arrived
if(totalClouds == clouds_modified.size())
{
pcl::PCLPointCloud2 merged_cloud = clouds[0];
clouds_modified[0] = false;
for(int i=1; i<clouds_modified.size(); ++i)
{
pcl::concatenatePointCloud(merged_cloud, clouds[i], merged_cloud);
clouds_modified[i] = false;
}
point_cloud_publisher_.publish(merged_cloud);
Eigen::MatrixXf points;
getPointCloudAsEigen(merged_cloud,points);
pointcloud_to_laserscan(points, &merged_cloud);
}
}
void LaserMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan, std::string topic)
{
sensor_msgs::PointCloud tmpCloud1,tmpCloud2;
tfListener_.waitForTransform(scan->header.frame_id.c_str(), destination_frame.c_str(), scan->header.stamp, ros::Duration(1));
projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, tmpCloud1, tfListener_);
try
{
tfListener_.transformPointCloud(destination_frame.c_str(), tmpCloud1, tmpCloud2);
}catch (tf::TransformException ex){ROS_ERROR("%s",ex.what());return;}
for(int i=0; i<input_topics.size(); ++i)
{
if(topic.compare(input_topics[i]) == 0)
{
sensor_msgs::convertPointCloudToPointCloud2(tmpCloud2,clouds[i]);
clouds_modified[i] = true;
}
}
int totalClouds = 0;
for(int i=0; i<clouds_modified.size(); ++i)
if(clouds_modified[i])
++totalClouds;
if(totalClouds == clouds_modified.size())
{
sensor_msgs::PointCloud2 merged_cloud = clouds[0];
clouds_modified[0] = false;
for(int i=1; i<clouds_modified.size(); ++i)
{
pcl::concatenatePointCloud(merged_cloud, clouds[i], merged_cloud);
clouds_modified[i] = false;
}
point_cloud_publisher_.publish(merged_cloud);
Eigen::MatrixXf points;
getPointCloudAsEigen(merged_cloud,points);
pointcloud_to_laserscan(points, &merged_cloud);
}
}
void PointCloud2ToPointCloud::Callback(const sensor_msgs::PointCloud2ConstPtr& input)
{
sensor_msgs::PointCloud output , output_tf;
sensor_msgs::convertPointCloud2ToPointCloud(*input,output);
if(!listener.waitForTransform( output.header.frame_id, "/odom",
input->header.stamp + ros::Duration().fromSec(0.1), ros::Duration(1.0))){
return;
}
try{
listener.transformPointCloud("/odom",output, output_tf);
pointcloud_pub.publish(output_tf);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
}
// Callback to register with tf::MessageFilter to be called when transforms are available
void cloud_cb(const boost::shared_ptr<const sensor_msgs::PointCloud2>& input)
{
sensor_msgs::PointCloud2 output;
try
{
tf_.transformPointCloud(target_frame_, *input, output);
pub_.publish(output);
/*
printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n",
point_out.point.x,
point_out.point.y,
point_out.point.z);
*/
}
catch (tf::TransformException &ex)
{
printf ("Failure %s\n", ex.what()); //Print exception which was caught
}
};
/*
* Filter robot out of the current scan
*/
void NiftiLaserFiltering::robot_filter(sensor_msgs::LaserScan& scan)
{
const double invalid = scan.range_max+1;
std::list<double> blacklisted;
sensor_msgs::PointCloud close_ptcld;
sensor_msgs::PointCloud transformed_ptcld;
close_ptcld.header = scan.header;
sensor_msgs::ChannelFloat32 index;
index.name = "index";
geometry_msgs::Point32 point;
point.z = 0;
double angle;
const ros::Time time = scan.header.stamp;
double max_dist = 0.7;
if (has_arm) { // when extended the end of the arm is further away
max_dist = 1.4;
}
double eps = 0.015;
// create a small point cloud with only the close points
for (unsigned int i=0; i<scan.ranges.size(); i++) {
if (scan.ranges[i]<max_dist) {
angle = scan.angle_min + i*scan.angle_increment;
point.x = scan.ranges[i]*cos(angle);
point.y = scan.ranges[i]*sin(angle);
close_ptcld.points.push_back(point);
index.values.push_back(i);
}
}
close_ptcld.channels.push_back(index);
//std::cout << close_ptcld.points.size() << std::endl;
double x, y, z;
// remove body
transformed_ptcld = close_ptcld;
for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
x = transformed_ptcld.points[i].x;
y = transformed_ptcld.points[i].y;
z = transformed_ptcld.points[i].z;
if ((fabs(y)<eps+0.15)&&(fabs(z)<eps+0.10)&&(x<eps+0.05))
blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
}
// remove left track
if (tf_listener.waitForTransform(laser_frame, "/left_track", time,
ros::Duration(1.))) {
tf_listener.transformPointCloud("/left_track", close_ptcld, transformed_ptcld);
for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
x = transformed_ptcld.points[i].x;
y = transformed_ptcld.points[i].y;
z = transformed_ptcld.points[i].z;
if
((fabs(y)<eps+(0.097/2))&&(fabs(x)<eps+0.09+(0.4997/2))&&(z>-eps-0.0705)&&(z<eps+0.1095))
blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
}
} else {
ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
" and /left_track for robot_filter (ignoring this part).");
}
// remove left track
if (tf_listener.waitForTransform(laser_frame, "/right_track", time,
ros::Duration(1.))) {
tf_listener.transformPointCloud("/right_track", close_ptcld, transformed_ptcld);
for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
x = transformed_ptcld.points[i].x;
y = transformed_ptcld.points[i].y;
z = transformed_ptcld.points[i].z;
if
((fabs(y)<eps+(0.097/2))&&(fabs(x)<eps+0.09+(0.4997/2))&&(z>-eps-0.0705)&&(z<eps+0.1095))
blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
}
} else {
ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
" and /right_track for robot_filter (ignoring this part).");
}
// remove front left flipper
if (tf_listener.waitForTransform(laser_frame, "/front_left_flipper", time,
ros::Duration(1.))) {
tf_listener.transformPointCloud("/front_left_flipper", close_ptcld, transformed_ptcld);
for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
x = transformed_ptcld.points[i].x;
y = transformed_ptcld.points[i].y;
z = transformed_ptcld.points[i].z;
if ((fabs(y)<eps+(0.050/2))&&(x>-eps-0.090)&&(x<eps+0.3476)&&
((x*sin(11.1*M_PI/180)+fabs(z)*cos(11.1*M_PI/180))<3*eps+0.090))
blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
}
} else {
ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
" and /front_left_flipper for robot_filter (ignoring this part).");
}
// remove front right flipper
if (tf_listener.waitForTransform(laser_frame, "/front_right_flipper", time,
ros::Duration(1.))) {
tf_listener.transformPointCloud("/front_right_flipper", close_ptcld, transformed_ptcld);
for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
x = transformed_ptcld.points[i].x;
y = transformed_ptcld.points[i].y;
z = transformed_ptcld.points[i].z;
if ((fabs(y)<eps+(0.050/2))&&(x>-eps-0.090)&&(x<eps+0.3476)&&
((x*sin(11.1*M_PI/180)+fabs(z)*cos(11.1*M_PI/180))<3*eps+0.090))
// Beware 3*eps !
blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
}
} else {
ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
" and /front_right_flipper for robot_filter (ignoring this part).");
}
// remove rear left flipper
if (tf_listener.waitForTransform(laser_frame, "/rear_left_flipper", time,
ros::Duration(1.))) {
tf_listener.transformPointCloud("/rear_left_flipper", close_ptcld, transformed_ptcld);
for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
x = transformed_ptcld.points[i].x;
y = transformed_ptcld.points[i].y;
z = transformed_ptcld.points[i].z;
if ((fabs(y)<eps+(0.050/2))&&(x>-eps-0.090)&&(x<eps+0.3476)&&
((x*sin(11.1*M_PI/180)+fabs(z)*cos(11.1*M_PI/180))<eps+0.090))
blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
}
} else {
ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
" and /rear_left_flipper for robot_filter (ignoring this part).");
}
// remove rear right flipper
if (tf_listener.waitForTransform(laser_frame, "/rear_right_flipper", time,
ros::Duration(1.))) {
tf_listener.transformPointCloud("/rear_right_flipper", close_ptcld, transformed_ptcld);
for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
x = transformed_ptcld.points[i].x;
y = transformed_ptcld.points[i].y;
z = transformed_ptcld.points[i].z;
if ((fabs(y)<eps+(0.050/2))&&(x>-eps-0.090)&&(x<eps+0.3476)&&
((x*sin(11.1*M_PI/180)+fabs(z)*cos(11.1*M_PI/180))<eps+0.090))
blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
}
} else {
ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
" and /rear_right_flipper for robot_filter (ignoring this part).");
}
// remove omnicam
if (tf_listener.waitForTransform(laser_frame, "/omnicam", time,
ros::Duration(1.))) {
tf_listener.transformPointCloud("/omnicam", close_ptcld, transformed_ptcld);
for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
x = transformed_ptcld.points[i].x;
y = transformed_ptcld.points[i].y;
z = transformed_ptcld.points[i].z;
if ((x*x+y*y<(eps+0.070)*(eps+0.070))&&(fabs(z)<eps+(0.15/2)))
blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
}
} else {
ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
" and /omnicam for robot_filter (ignoring this part).");
}
// remove arm
if (has_arm) {
bool no_arm = true;
// upper arm
if (tf_listener.waitForTransform(laser_frame, "/shoulder", time,
ros::Duration(1.))) {
no_arm = false;
tf_listener.transformPointCloud("/shoulder", close_ptcld, transformed_ptcld);
for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
x = transformed_ptcld.points[i].x;
y = transformed_ptcld.points[i].y;
z = transformed_ptcld.points[i].z;
if ((y*y+z*z<(eps+0.050/2)*(eps+0.050/2))&&(fabs(x-0.500/2)<eps+(0.500/2)))
blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
}
} else {
ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
" and /shoulder for arm of robot_filter (ignoring this part).");
}
// elbow + forearm
if (tf_listener.waitForTransform(laser_frame, "/elbow", time,
ros::Duration(1.))) {
no_arm = false;
tf_listener.transformPointCloud("/elbow", close_ptcld, transformed_ptcld);
for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
x = transformed_ptcld.points[i].x;
y = transformed_ptcld.points[i].y;
z = transformed_ptcld.points[i].z;
// elbow
if ((x*x+z*z<(eps+0.052/2)*(eps+0.052/2))&&(fabs(y+(0.122/2-0.029))<eps+(0.122/2)))
blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
// forearm
if (((y+0.064)*(y+0.064)+z*z<(eps+0.050/2)*(eps+0.050/2))&&(fabs(x-0.450/2)<eps+(0.450/2)))
blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
}
} else {
ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
" and /elbow for arm of robot_filter (ignoring this part).");
}
// pan tilt
if (tf_listener.waitForTransform(laser_frame, "/ptu_joint", time,
ros::Duration(1.))) {
no_arm = false;
tf_listener.transformPointCloud("/ptu_joint", close_ptcld, transformed_ptcld);
for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
x = transformed_ptcld.points[i].x;
y = transformed_ptcld.points[i].y;
z = transformed_ptcld.points[i].z;
if ((x*x+y*y+z*z<(eps+0.120)*(eps+0.120)))
blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
}
} else {
ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
" and /ptu_joint for arm of robot_filter (ignoring this part).");
}
if (no_arm) {
arm_failures += 1;
if (arm_failures>50) {
ROS_WARN_STREAM("Too many failures to find the arm /tf: disabling arm filtering.");
has_arm = false;
}
} else {
arm_failures = 0;
}
}
// remove blacklisted points
//std::cout << blacklisted.size() << " -> ";
//blacklisted.sort(); // don't know if that's necessary
//blacklisted.unique();
//std::cout << blacklisted.size()<< std::endl;
for (std::list<double>::const_iterator it=blacklisted.begin();it!=blacklisted.end();++it) {
scan.ranges[int(*it)] = invalid;
}
}
void GridConstructionNode::scanCallback (const sm::LaserScan& scan)
{
try {
// We'll need the transform between sensor and fixed frames at the time when the scan was taken
if (!tf_.waitForTransform(fixed_frame_, sensor_frame_, scan.header.stamp, ros::Duration(1.0)))
{
ROS_WARN_STREAM ("Timed out waiting for transform from " << sensor_frame_ << " to "
<< fixed_frame_ << " at " << scan.header.stamp.toSec());
return;
}
// Figure out current sensor position
tf::Pose identity(tf::createIdentityQuaternion(), tf::Vector3(0, 0, 0));
tf::Stamped<tf::Pose> odom_pose;
tf_.transformPose(fixed_frame_, tf::Stamped<tf::Pose> (identity, ros::Time(), sensor_frame_), odom_pose);
// Project scan from sensor frame (which varies over time) to odom (which doesn't)
sm::PointCloud fixed_frame_cloud;
laser_geometry::LaserProjection projector_;
projector_.transformLaserScanToPointCloud (fixed_frame_, scan, fixed_frame_cloud, tf_);
// Now transform back into sensor frame at a single time point
sm::PointCloud sensor_frame_cloud;
tf_.transformPointCloud (sensor_frame_, scan.header.stamp, fixed_frame_cloud, fixed_frame_,
sensor_frame_cloud);
// Construct and save LocalizedCloud
CloudPtr loc_cloud(new gu::LocalizedCloud());
loc_cloud->cloud.points = sensor_frame_cloud.points;
tf::poseTFToMsg(odom_pose, loc_cloud->sensor_pose);
loc_cloud->header.frame_id = fixed_frame_;
Lock lock(mutex_);
last_cloud_=loc_cloud;
}
catch (tf::TransformException& e) {
ROS_INFO ("Not saving scan due to tf lookup exception: %s",
e.what());
}
}
