Skip to content

Commit

Permalink
Fixed an enormous amount of bugs, optimized and cleaned up the code...
Browse files Browse the repository at this point in the history
  • Loading branch information
@gstavrinos
gstavrinos committed Dec 4, 2019
1 parent ad6c8f9 commit f17508a
Showing 1 changed file with 86 additions and 115 deletions.
201 changes: 86 additions & 115 deletions src/laserscan_stacker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,150 +10,121 @@
#include <sensor_msgs/point_cloud_conversion.h>
#include <pointcloud_msgs/PointCloud2_Segments.h>

double factor;
int size, overlap;
ros::Publisher pub;
std::deque<sensor_msgs::PointCloud> v;

using namespace std;
laser_geometry::LaserProjection projector;

tf::TransformListener* tfListener;

pointcloud_msgs::PointCloud2_Segments bufferToAccumulator(const sensor_msgs::LaserScan::ConstPtr scan_in){
sensor_msgs::PointCloud2 accumulator;

std::string input_topic, out_topic, base_link_frame;
double middle_z = 0.0;
double z_diff = 0.0;
int last_valid_index = -1;
int tmp_index = -1;

class LaserscanStacker{

public:
int size, overlap, cnt, num_scans;
float ang_min , ang_max , ang_incr , time_incr;
float rng_min , rng_max , scan_tm;
double factor;

std::string frame_id;
ros::Publisher pub;
ros::NodeHandle node;
ros::Time r_time;
std::deque<sensor_msgs::PointCloud> v;

laser_geometry::LaserProjection projector;
message_filters::Subscriber<sensor_msgs::LaserScan> laser_sub;

tf::TransformListener tfListener;
tf::MessageFilter<sensor_msgs::LaserScan> laser_transform;

LaserscanStacker(ros::NodeHandle n) :
node(n),
laser_sub(node, input_topic, 10),
laser_transform(laser_sub, tfListener, base_link_frame, 10)
{
laser_transform.registerCallback(boost::bind(&LaserscanStacker::scanCallback, this, _1));
laser_transform.setTolerance(ros::Duration(0.01));
pub = node.advertise<pointcloud_msgs::PointCloud2_Segments> (out_topic, 5);
}


pointcloud_msgs::PointCloud2_Segments bufferToAccumulator(std::deque<sensor_msgs::PointCloud> v_, ros::Time rt){
sensor_msgs::PointCloud2 accumulator;

double middle_z = 0.0;
double z_diff = 0.0;
int last_valid_index = -1;
int tmp_index = -1;

for (unsigned j=0; j < v_.size(); j++) {
if (j > 0) {
tmp_index = -1;
if (v_[j-1].points.size()) {
tmp_index = j-1;
}
else if (last_valid_index >= 0) {
tmp_index = last_valid_index;
}
if (tmp_index >= 0) {
z_diff = std::max(0.0, v_[tmp_index].points[0].z + ros::Duration(v_[j].header.stamp - v_[tmp_index].header.stamp).toSec() * factor);
for (size_t i=0; i < v_[j].points.size(); i++) {
v_[j].points[i].z = z_diff;
}
if (v_[j].points.size()) {
last_valid_index = j;
}
for (unsigned j=0; j < v.size(); j++) {
if (j > 0) {
tmp_index = -1;
if (v[j-1].points.size()) {
tmp_index = j-1;
}
else if (last_valid_index >= 0) {
tmp_index = last_valid_index;
}
if (tmp_index >= 0) {
z_diff = std::max(0.0, v[tmp_index].points[0].z + ros::Duration(v[j].header.stamp - v[tmp_index].header.stamp).toSec() * factor);
for (size_t i=0; i < v[j].points.size(); i++) {
v[j].points[i].z = z_diff;
}
if (j == v_.size() / 2 and v_[j].points.size() and v_[j].points[0].z > middle_z) {
middle_z = v_[j].points[0].z;
if (v[j].points.size()) {
last_valid_index = j;
}
}
sensor_msgs::PointCloud2 pc2;
sensor_msgs::convertPointCloudToPointCloud2(v_[j], pc2);
pcl::concatenatePointCloud( accumulator, pc2, accumulator);
if (j == v.size() / 2 and v[j].points.size() and v[j].points[0].z > middle_z) {
middle_z = v[j].points[0].z;
}
}

pointcloud_msgs::PointCloud2_Segments c;

c.header.stamp = ros::Time::now();
c.clusters.push_back(accumulator);
c.factor = factor;
c.overlap = overlap ;
if (v_.size()) { // this should not be needed, but bsts
c.first_stamp = v_[0].header.stamp;
else {
for (size_t i=0; i < v[j].points.size(); i++) {
v[j].points[i].z = 0.f;
}
}
c.header.frame_id = frame_id;
c.num_scans = num_scans;
c.angle_min = ang_min;
c.angle_max = ang_max;
c.angle_increment = ang_incr;
c.range_min = rng_min;
c.range_max = rng_max;
c.scan_time = scan_tm;
c.rec_time = rt;
c.middle_z = middle_z;
c.idForTracking = -1;

return c;

sensor_msgs::PointCloud2 pc2;
sensor_msgs::convertPointCloudToPointCloud2(v[j], pc2);
pcl::concatenatePointCloud( accumulator, pc2, accumulator);
}

pointcloud_msgs::PointCloud2_Segments c;

void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in){
sensor_msgs::PointCloud2 cloud;
try{
projector.transformLaserScanToPointCloud(base_link_frame, *scan_in, cloud, tfListener, 1000.0);
c.header.stamp = ros::Time::now();
c.clusters.push_back(accumulator);
c.factor = factor;
c.overlap = overlap ;
if (v.size()) { // this should not be needed, but bsts
c.first_stamp = v[0].header.stamp;
}
c.header.frame_id = scan_in->header.frame_id;
//c.num_scans = num_scans;
c.angle_min = scan_in->angle_min;
c.angle_max = scan_in->angle_max;
c.angle_increment = scan_in->angle_increment;
c.range_min = scan_in->range_min;
c.range_max = scan_in->range_max;
c.scan_time = scan_in->scan_time;
c.rec_time = scan_in->header.stamp;
c.middle_z = middle_z;
c.idForTracking = -1;

return c;
}

sensor_msgs::PointCloud pc1;
sensor_msgs::convertPointCloud2ToPointCloud (cloud, pc1);

v.push_back(pc1);
void scanCallback (const sensor_msgs::LaserScan::ConstPtr scan_in){
sensor_msgs::PointCloud2 cloud;
try{
projector.transformLaserScanToPointCloud(scan_in->header.frame_id, *scan_in, cloud, *tfListener,
laser_geometry::channel_option::Intensity |
laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance |
laser_geometry::channel_option::Timestamp |
laser_geometry::channel_option::Viewpoint);

r_time = scan_in->header.stamp;
sensor_msgs::PointCloud pc1;
sensor_msgs::convertPointCloud2ToPointCloud (cloud, pc1);

if (v.size() > size){
pub.publish(bufferToAccumulator(v, r_time));
v.erase(v.begin(), v.begin() + overlap);
}
v.push_back(pc1);

frame_id = scan_in->header.frame_id;
ang_min = scan_in->angle_min;
ang_max = scan_in->angle_max ;
ang_incr = scan_in->angle_increment;
time_incr = scan_in->time_increment;
rng_min = scan_in->range_min;
rng_max = scan_in->range_max;
scan_tm = scan_in->scan_time;
if (v.size() == size){
pub.publish(bufferToAccumulator(scan_in));
v.erase(v.begin(), v.begin() + overlap);
}
catch(...){}
}
catch(...){}
}

};

int main(int argc, char** argv){

ros::init(argc, argv, "laserscan_stacker");
ros::NodeHandle n;

n.param("laserscan_stacker/out_topic", out_topic, std::string("laserscan_stacker/scans"));
n.param("laserscan_stacker/input_topic", input_topic, std::string("/scan"));
n.param("laserscan_stacker/base_link_frame", base_link_frame, std::string("base_link"));
std::string input_topic, out_topic;

LaserscanStacker lstopc(n);
tfListener = new (tf::TransformListener);

n.param("laserscan_stacker/size", lstopc.size, 40);
n.param("laserscan_stacker/factor", lstopc.factor, 5.0);
n.param("laserscan_stacker/overlap", lstopc.overlap, 35);
n.param("laserscan_stacker/out_topic", out_topic, std::string("laserscan_stacker/scans"));
n.param("laserscan_stacker/input_topic", input_topic, std::string("/scan"));
n.param("laserscan_stacker/size", size, 40);
n.param("laserscan_stacker/factor", factor, 5.0);
n.param("laserscan_stacker/overlap", overlap, 35);

ros::Subscriber s = n.subscribe(input_topic, 1, scanCallback);
pub = n.advertise<pointcloud_msgs::PointCloud2_Segments> (out_topic, 1);

ros::spin();

Expand Down

0 comments on commit f17508a

Please sign in to comment.