Hello there,
I have been trying to convert a PointCloud topic which is generated by gazebo_ros_openni_kinect into LaserScan topic. The gazebo plugin publishes a simulated openni output to /cam3d/depth/points topic, and it seems to work correctly.
I was able to inspect the code that pcl_to_scan package has, and managed to rebuilt it into a standalone working source file using the ros kinect publisher tutorials and disabling dynamic reconfigure. The file publishes some laser data. However, the laser data seems to decrease over time, and when it reaches 0, it starts over.
For example, when the data is first published: (Range values are around 0.8)>> angle_min: -1.57079637051 angle_max:>> 1.57079637051 angle_increment: 0.0174532923847 time_increment: 0.0 scan_time: 0.0333333350718 range_min:>> 0.10000000149 range_max: 10.0 ranges: [0.8371890783309937,>> 0.8371157646179199, 0.8371505737304688, 0.841155469417572, 0.8416852355003357, 0.8424785137176514, 0.7871079444885254, 0.7877516150474548, 0.7892191410064697, 0.7909254431724548, 0.7928680777549744, 0.7950447797775269, 0.7974526882171631, 0.8000890612602234, 0.8029510378837585, 0.8060353398323059, 0.8093387484550476, 0.8137710094451904, 0.8175548315048218, 0.8215464353561401, 0.8268224000930786, 0.831267774105072, 0.8370987176895142, 0.8419783115386963, 0.8483396172523499, 0.8549846410751343, 0.8604996800422668, 0.8676356673240662, 0.8750333786010742, 0.882685124874115, 0.892191469669342, 0.900374710559845, 0.9087872505187988, 0.9191746711730957, 0.9280658960342407, 0.939006507396698, 0.9502314925193787, 0.9617291688919067, 0.9734877347946167, 0.9875211715698242, 0.9998075366020203, 1.014427661895752, 1.0293407440185547, 1.0445315837860107, 1.0622141361236572, 1.0779519081115723, 1.0962239503860474, 1.1171224117279053, 1.1359834671020508, 1.157501220703125, 1.1817551851272583, 1.2038718461990356, 1.2287416458129883, 1.256429672241211, 1.2844353914260864, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0]And after a while, it is like this: (Range values are around 0.02)>> angle_min: -1.57079637051 angle_max:>> 1.57079637051 angle_increment: 0.0174532923847 time_increment: 0.0 scan_time: 0.0333333350718 range_min:>> 0.10000000149 range_max: 10.0 ranges: [0.11726024001836777,>> 0.05766232684254646, 0.04177795350551605, 0.02987378090620041, 0.025909584015607834, 0.021949056535959244, 0.017994627356529236, 0.046077899634838104, 0.01405144389718771, 0.0584421269595623, 0.03429332748055458, 0.010132638737559319, 0.026504002511501312, 0.04287064075469971, 0.03901761397719383, 0.022650254890322685, 0.035191018134355545, 0.060264796018600464, 0.006283995695412159, 0.05273519456386566, 0.04019985720515251, 0.027660993859171867, 0.036499835550785065, 0.015129179693758488, 0.023996248841285706, 0.03286304697394371, 0.09480049461126328, 0.0381767563521862, 0.02931303158402443, 0.020446542650461197, 0.01158352755010128, 0.04898853227496147, 0.02588552236557007, 0.05444787070155144, 0.03138109669089317, 0.017083730548620224, 0.0369064100086689, 0.022636234760284424, 0.04800030216574669, 0.02820809744298458, 0.03378802165389061, 0.04495486989617348, 0.05611952021718025, 0.08399122208356857, 0.1616191416978836, 0.0028102879878133535, 0.08396531641483307, 0.05610218644142151, 0.044940974563360214, 0.033777572214603424, 0.028199369087815285, 0.047970641404390335, 0.022629227489233017, 0.03688358888030052, 0.017078444361686707, 0.03136168420314789, 0.054380614310503006, 0.02586950734257698, 0.048928000032901764, 0.011579941026866436, 0.020433885976672173, 0.029285836964845657, 0.03812955319881439, 0.09447945654392242, 0.03282240405678749, 0.02397397719323635, 0.01511981338262558, 0.036443427205085754, 0.027626775205135345, 0.040125325322151184, 0.05260495841503143, 0.006282049231231213, 0.06007887050509453, 0.03512575477361679, 0.022622225806117058, 0.03893321752548218, 0.04276470094919205, 0.026463015004992485, 0.01012636348605156, 0.034219127148389816, 0.05822576582431793, 0.014038392342627048, 0.04593560844659805, 0.017972351983189583, 0.021915104240179062, 0.025861503556370735, 0.029809128493070602, 0.041648901998996735, 0.05741326883435249, 0.11621594429016113, 0.11621594429016113, 0.05741326883435249, 0.041648901998996735, 0.029809124767780304, 0.025861501693725586, 0.021915104240179062, 0.017972351983189583, 0.045935604721307755, 0.014038392342627048, 0.05822575092315674, 0.03421912342309952, 0.01012636348605156, 0.026463011279702187, 0.04276468977332115, 0.03893321007490158, 0.02262222208082676, 0.035125747323036194, 0.060078851878643036, 0.006282049231231213, 0.05260493978857994, 0.04012531787157059, 0.027626771479845047, 0.03644341602921486, 0.015119810588657856, 0.023973971605300903, 0.032822392880916595, 11.0, 0.0381295420229435, 0.029285825788974762, 0.020433882251381874, 0.011579939164221287, 11.0, 0.025869499891996384, 11.0, 0.03136167302727699, 0.017078440636396408, 0.03688357397913933, 0.02262921817600727, 11.0, 0.02819935604929924, 0.03377755358815193, 11.0, 11.0, 11.0, 11.0, 0.0028102879878133535, 11.0, 11.0, 11.0, 11.0, 0.028208084404468536, 11.0, 0.02263622358441353, 11.0, 0.017083728685975075, 11.0, 11.0, 0.025885511189699173, 11.0, 0.011583525687456131, 0.02044653333723545, 11.0, 11.0, 11.0, 11.0, 0.02399623766541481, 0.015129175037145615, 11.0, 11.0, 11.0, 11.0, 0.006283994298428297, 11.0, 11.0, 0.02265024557709694, 11.0, 11.0, 11.0, 0.01013263687491417, 11.0, 11.0, 0.014051438309252262, 11.0, 0.01799461990594864, 0.021949047222733498, 11.0, 11.0, 11.0, 11.0, 11.0]Here is the code that I'm trying to work with:
#include
#include
#include "pluginlib/class_list_macros.h"
#include "nodelet/nodelet.h"
#include "sensor_msgs/LaserScan.h"
#include "pcl/point_cloud.h"
#include "pcl_ros/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl/ros/conversions.h"
#include
// Dynamic reconfigure includes.
#include
using namespace std;
typedef pcl::PointCloud PointCloud;
ros::Publisher pub_;
ros::Subscriber sub_;
ros::Publisher marker_pub;
void callback(const PointCloud::ConstPtr& msg)
{
sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
ROS_INFO("Got cloud");
//Copy Header
output->header = msg->header;
output->header.frame_id = "scan"; //output_frame_id_
output->angle_min = -M_PI / 2; // Default: -M_PI / 6;
output->angle_max = M_PI / 2; // Default: M_PI / 6;;
output->angle_increment = M_PI / 180.0; // Default: 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 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;
double max_height_ = 5; // Default: 5
double min_height_ = 0; // Default: 0
BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points){
float x = pt.x;
float y = pt.y;
float z = pt.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;
if (std::isnan(x) || std::isnan(y) || std::isnan(z)) {
ROS_INFO("rejected for nan in point(%f, %f, %f)\n", x, y, z);
continue;
}
if (z > max_height_ || z < min_height_) {
ROS_INFO("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) {
ROS_INFO("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);
ROS_INFO("Y: %f %f, Z: %f %f", min_y, max_y, min_z, max_z);
line_list.header = msg->header;
line_list.header.frame_id = output->header.frame_id;
line_list.ns = "points_and_lines";
line_list.action = visualization_msgs::Marker::ADD;
line_list.pose.orientation.w = 0.0; // Default: 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);
}
}
double min_height_, max_height_;
int32_t u_min_, u_max_;
std::string output_frame_id_;
bool dynamic_set;
int main(int argc, char** argv)
{
ros::init(argc, argv, "sub_pcl");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/cam3d/depth/points", 1, callback);
pub_ = nh.advertise ("/scan", 10);
marker_pub = nh.advertise (
"visualization_marker", 10);
ros::spin();
}
I'm really stuck and don't know why the range values decreases. I suspect that it might have something to do with poorly configured TF, but I don't know how to change these parameters in order to correct the code. Any help will be greately appreciated.
↧