void callback(const PointCloud::ConstPtr& cloud) {
sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
NODELET_DEBUG("Got cloud");
//Copy Header
output->header = cloud->header;
output->header.frame_id = output_frame_id_;
output->angle_min = -M_PI / 6;
output->angle_max = M_PI / 6;
output->angle_increment = M_PI / 180.0 / 2.0;
output->time_increment = 0.0;
output->scan_time = 1.0 / 30.0;
output->range_min = 0.1;
output->range_max = 10.0;
uint32_t ranges_size = std::ceil(
(output->angle_max - output->angle_min)
/ output->angle_increment);
output->ranges.assign(ranges_size, output->range_max + 1.0);
//pcl::PointCloud<pcl::PointXYZ> cloud;
//pcl::fromROSMsg(*input, cloud);
visualization_msgs::Marker line_list;
float min_z = 100;
float max_z = -100;
float min_y = 100;
float max_y = -100;
// NODELET_INFO("New scan...");
for (PointCloud::const_iterator it = cloud->begin(); it != cloud->end(); ++it) {
const float &x = it->x;
const float &y = it->y;
const float &z = it->z;
if (z < min_z)
min_z = z;
if (z > max_z)
max_z = z;
if (y < min_y)
min_y = y;
if (y > max_y)
max_y = y;
/* for (uint32_t u = std::max((uint32_t)u_min_, 0U); u < std::min((uint32_t)u_max_, cloud.height - 1); u++)
for (uint32_t v = 0; v < cloud.width -1; v++)
{
//NODELET_ERROR("%d %d, %d %d", u, v, cloud.height, cloud.width);
pcl::PointXYZ point = cloud.at(v,u); ///WTF Column major?
const float &x = point.x;
const float &y = point.y;
const float &z = point.z;
*/
if (std::isnan(x) || std::isnan(y) || std::isnan(z)) {
NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y,
z);
continue;
}
if (z > max_height_ || z < min_height_) {
NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n",
z, min_height_, max_height_);
continue;
}
double angle = atan2(y, x);
if (angle < output->angle_min || angle > output->angle_max) {
NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n",
angle, output->angle_min, output->angle_max);
continue;
}
int index = (angle - output->angle_min) / output->angle_increment;
// NODELET_INFO("index xyz( %f %f %f) angle %f index %d", x, y, z, angle, index);
double range_sq = y * y + x * x;
if (output->ranges[index] * output->ranges[index] > range_sq)
output->ranges[index] = sqrt(range_sq);
}
NODELET_INFO("Y: %f %f, Z: %f %f", min_y, max_y, min_z, max_z);
line_list.header = cloud->header;
line_list.header.frame_id = output_frame_id_;
line_list.ns = "points_and_lines";
line_list.action = visualization_msgs::Marker::ADD;
line_list.pose.orientation.w = 1.0;
line_list.id = 0;
line_list.type = visualization_msgs::Marker::LINE_LIST;
line_list.scale.x = 0.1;
// line_list.color.r = 1.0;
line_list.color.a = 1.0;
// Create the vertices for the points and lines
for (uint32_t i = 0; i < ranges_size; ++i) {
float rng = output->ranges[i];
float a = output->angle_min + i * output->angle_increment;
float x = rng * cos(a);
float y = rng * sin(a);
geometry_msgs::Point p;
std_msgs::ColorRGBA col;
p.x = x;
p.y = y;
p.z = min_height_;
col.g = rng / (output->range_max);
col.r = 1.0 - col.g;
line_list.colors.push_back(col);
line_list.colors.push_back(col);
// The line list needs two points for each line
line_list.points.push_back(p);
p.z = max_height_;
line_list.points.push_back(p);
}
marker_pub.publish(line_list);
pub_.publish(output);
}
void callback(const PointCloud::ConstPtr& cloud)
{
sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
NODELET_DEBUG("Got cloud");
//Copy Header
output->header = cloud->header;
output->header.frame_id = output_frame_id_;
output->angle_min = -M_PI/2;
output->angle_max = M_PI/2;
output->angle_increment = M_PI/180.0/2.0;
output->time_increment = 0.0;
output->scan_time = 1.0/30.0;
output->range_min = 0.45;
output->range_max = 10.0;
uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment);
output->ranges.assign(ranges_size, output->range_max + 1.0);
//pcl::PointCloud<pcl::PointXYZ> cloud;
//pcl::fromROSMsg(*input, cloud);
for (PointCloud::const_iterator it = cloud->begin(); it != cloud->end(); ++it)
{
const float &x = it->x;
const float &y = it->y;
const float &z = it->z;
/* for (uint32_t u = std::max((uint32_t)u_min_, 0U); u < std::min((uint32_t)u_max_, cloud.height - 1); u++)
for (uint32_t v = 0; v < cloud.width -1; v++)
{
//NODELET_ERROR("%d %d, %d %d", u, v, cloud.height, cloud.width);
pcl::PointXYZ point = cloud.at(v,u); ///WTF Column major?
const float &x = point.x;
const float &y = point.y;
const float &z = point.z;
*/
if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
{
NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
continue;
}
if (-y > max_height_ || -y < min_height_)
{
NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", x, min_height_, max_height_);
continue;
}
double angle = -atan2(x, z);
if (angle < output->angle_min || angle > output->angle_max)
{
NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max);
continue;
}
int index = (angle - output->angle_min) / output->angle_increment;
//printf ("index xyz( %f %f %f) angle %f index %d\n", x, y, z, angle, index);
double range_sq = z*z+x*x;
if (output->ranges[index] * output->ranges[index] > range_sq)
output->ranges[index] = sqrt(range_sq);
}
pub_.publish(output);
}