void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
sensor_msgs::PointCloud2 raw_cloud_msg;
sensor_msgs::PointCloud2 local_cloud_msg;
sensor_msgs::PointCloud2 global_cloud_msg;
PointCloudPtr_t scale_cloud(new PointCloud_t());
PointCloudPtr_t local_cloud(new PointCloud_t());
PointCloudPtr_t global_cloud(new PointCloud_t());
projector_.projectLaser(*scan, raw_cloud_msg);
pcl::fromROSMsg(raw_cloud_msg, *scale_cloud);
for(int i=0; i<scale_cloud->size(); i++)
{
pcl::PointXYZ& point = scale_cloud->at(i);
point.z = std::floor(point.z);
}
this->filterGlobal(scale_cloud, global_cloud);
this->filterLocal(scale_cloud, local_cloud);
pcl::toROSMsg(*global_cloud, global_cloud_msg);
pcl::toROSMsg(*local_cloud, local_cloud_msg);
this->local_cloud_publisher_.publish(local_cloud_msg);
this->global_cloud_publisher_.publish(global_cloud_msg);
}
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
sensor_msgs::LaserScan scanData = *msg;
auto rangeIsValid = [&scanData](float range) {
return !std::isnan(range) && range > scanData.range_min && range < scanData.range_max;
};
for (unsigned int i = 0; i < scanData.ranges.size(); i++)
{
float& range = scanData.ranges[i];
if (range > scanData.range_max || range < scanData.range_min)
continue;
// Too close
if (range < min_dist)
range = scanData.range_max + 1;
// No valid neighbors
else if ((i == 0 || !rangeIsValid(scanData.ranges[i - 1])) &&
(i == (scanData.ranges.size() - 1) || !rangeIsValid(scanData.ranges[i + 1])))
range = scanData.range_max + 1;
// No close neighbors
else if ((i == 0 || abs(scanData.ranges[i - 1] - range) > neighbor_dist) &&
(i == (scanData.ranges.size() - 1) || abs(scanData.ranges[i + 1] - range) > neighbor_dist))
range = scanData.range_max + 1;
}
sensor_msgs::PointCloud2 cloud;
projection.projectLaser(scanData, cloud);
cloud.header.frame_id = "/lidar";
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for_pub(new pcl::PointCloud<pcl::PointXYZ>());
fromROSMsg(cloud, *cloud_for_pub);
_pointcloud_pub.publish(*cloud_for_pub);
}
void laser_receive(const sensor_msgs::LaserScan::ConstPtr& reading) {
sensor_msgs::PointCloud cloud;
projector_.projectLaser(*reading, cloud);
adjustByIMUData(cloud);
std::vector<point2d_t> target;
for(int i = 0; i < cloud.points.size(); ++i) {
point2d_t pt;
pt.x = cloud.points[i].x;
pt.y = cloud.points[i].y;
target.push_back(pt);
}
// build tree for search
if(reference.size() != 0) {
tree2d_t *tree = tree2d_build(reference.begin(), reference.end(), X, depth_of_tree);
if (!tree) {
std::cout << "failed!" << std::endl;
return;
}
reference = target;
// find pose
pose_t pose;
pose.p.x = 0;
pose.p.y = 0;
pose.yaw = 0;
pose = icp_align(tree, target, pose, iterations);
std::cout << pose << std::endl;
pose_estimator::pose_estimator_msg outMsg;
outMsg.x = pose.p.x;
outMsg.y = pose.p.y;
outMsg.yaw = pose.yaw;
pub.publish(outMsg);
// free tree
tree2d_free(&tree);
if (tree) {
std::cout << "failed!" << std::endl;
return;
}
} else {
reference = target;
}
}
void callback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
sensor_msgs::LaserScan scan_filtered=*scan_in;
int total_points=(scan_filtered.angle_max-scan_filtered.angle_min)/scan_filtered.angle_increment;
for (int i=0;i<total_points;i++)
{
if(scan_filtered.ranges[i]==0)
{
scan_filtered.ranges[i]=5;
}
}
projector_.projectLaser(scan_filtered,cloud);
pcl_pub.publish(cloud);
}
void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
sensor_msgs::PointCloud2 cloud;
try
{
//projector_.transformLaserScanToPointCloud(
// "laser",*scan_in, cloud,listener_);
projector_.projectLaser(*scan_in,cloud);
}
catch (tf::TransformException& e)
{
std::cout << e.what();
return;
}
// Do something with cloud.
for(unsigned int i=0; i<cloud.fields.size(); i++)
{cloud.fields[i].count = 1;}
if (::frame_count==0)
{
wall=cloud;
scan_pub_.publish(wall); //save 1st pointcloud as wall sensor_msg
ROS_INFO("published wall");
}
else{
scan_pub_.publish(cloud);
//ROS_INFO("published frame %f ",::frame_count);
}
::frame_count++;
}
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
sensor_msgs::LaserScan scanData = *msg;
auto rangeIsValid = [&scanData](float range) {
return !std::isnan(range) && range > scanData.range_min && range < scanData.range_max;
};
for(auto i = 0; i < scanData.ranges.size(); i++)
{
float& range = scanData.ranges[i];
if(range > scanData.range_max || range < scanData.range_min)
continue;
// Too close
if(range < 0.4)
range = scanData.range_max + 1;
// Too far
else if(range > 6.0)
range = scanData.range_max + 1;
// No valid neighbors
else if((i == 0 || !rangeIsValid(scanData.ranges[i-1])) && (i == (scanData.ranges.size()-1) || !rangeIsValid(scanData.ranges[i+1])))
range = scanData.range_max + 1;
// No close neighbors
else if((i == 0 || abs(scanData.ranges[i-1] - range) > 0.2) && (i == (scanData.ranges.size()-1) || abs(scanData.ranges[i+1] - range) > 0.2))
range = scanData.range_max + 1;
}
sensor_msgs::PointCloud2 cloud;
projection.projectLaser(scanData, cloud);
cloud.header.frame_id = "/lidar";
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for_pub(new pcl::PointCloud<pcl::PointXYZ>());
fromROSMsg(cloud, *cloud_for_pub);
cloud_for_pub->push_back(pcl::PointXYZ(-3,0,0)); //really solid non-hacky fix for some unknown error
_pointcloud_pub.publish(*cloud_for_pub);
}