laserscan_virtualizer.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <string.h>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>
#include <laser_geometry/laser_geometry.h>
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include "sensor_msgs/LaserScan.h"
#include <pcl_ros/point_cloud.h>
#include <Eigen/Dense>
#include <dynamic_reconfigure/server.h>
#include <ira_laser_tools/laserscan_virtualizerConfig.h>

typedef pcl::PointCloud<pcl::PointXYZ> myPointCloud;

using namespace std;
using namespace pcl;
using namespace laserscan_virtualizer;

class LaserscanVirtualizer
{
public:
        LaserscanVirtualizer();
        void scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan, std::string topic);
        void pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLHeader scan_header, int pub_index);
        void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &pcl_in);
        void reconfigureCallback(laserscan_virtualizerConfig &config, uint32_t level);

private:
        ros::NodeHandle node_;
        tf::TransformListener tfListener_;
        vector<tf::StampedTransform> transform_;

        ros::Subscriber point_cloud_subscriber_;
        vector<ros::Publisher> virtual_scan_publishers;
        vector<string> output_frames;

        void virtual_laser_scan_parser();

        double angle_min;
        double angle_max;
        double angle_increment;
        double time_increment;
        double scan_time;
        double range_min;
        double range_max;

        string cloud_frame;
        string base_frame;
        string cloud_topic;
        string output_laser_topic;
        string virtual_laser_scan;
};

void LaserscanVirtualizer::reconfigureCallback(laserscan_virtualizerConfig &config, uint32_t level)
{
        this->angle_min = config.angle_min;
        this->angle_max = config.angle_max;
        this->angle_increment = config.angle_increment;
        this->time_increment = config.time_increment;
        this->scan_time = config.scan_time;
        this->range_min = config.range_min;
        this->range_max = config.range_max;
}

void LaserscanVirtualizer::virtual_laser_scan_parser()
{
        // LaserScan frames to use for virtualization
        istringstream iss(virtual_laser_scan);
        vector<string> tokens;
        copy(istream_iterator<string>(iss), istream_iterator<string>(), back_inserter<vector<string>>(tokens));

        vector<string> tmp_output_frames;

        for (int i = 0; i < tokens.size(); i++)
        {
                ros::Time beg = ros::Time::now();
                if (tfListener_.waitForTransform(base_frame, tokens[i], ros::Time(0), ros::Duration(1.0))) // Check if TF knows the transform from this frame reference to base_frame reference
                {
                        cout << "Elapsed: " << ros::Time::now() - beg << endl;
                        cout << "Adding: " << tokens[i] << endl;
                        tmp_output_frames.push_back(tokens[i]);
                }
                else
                        cout << "Can't transform: '" << tokens[i] + "' to '" << base_frame << "'" << endl;
        }

        // Sort and remove duplicates
        sort(tmp_output_frames.begin(), tmp_output_frames.end());
        std::vector<string>::iterator last = std::unique(tmp_output_frames.begin(), tmp_output_frames.end());
        tmp_output_frames.erase(last, tmp_output_frames.end());

        // Do not re-advertize if the topics are the same
        if ((tmp_output_frames.size() != output_frames.size()) || !equal(tmp_output_frames.begin(), tmp_output_frames.end(), output_frames.begin()))
        {
                // Shutdown previous publishers
                for (int i = 0; i < virtual_scan_publishers.size(); i++)
                        virtual_scan_publishers[i].shutdown();

                cloud_frame = "";

                output_frames = tmp_output_frames;
                if (output_frames.size() > 0)
                {
                        virtual_scan_publishers.resize(output_frames.size());
                        ROS_INFO("Publishing: %ld virtual scans", virtual_scan_publishers.size());
                        cout << "Advertising topics: " << endl;
                        for (int i = 0; i < output_frames.size(); ++i)
                        {
                                if (output_laser_topic.empty())
                                {
                                        virtual_scan_publishers[i] = node_.advertise<sensor_msgs::LaserScan>(output_frames[i].c_str(), 1);
                                        cout << "\t\t" << output_frames[i] << " on topic " << output_frames[i].c_str() << endl;
                                }
                                else
                                {
                                        virtual_scan_publishers[i] = node_.advertise<sensor_msgs::LaserScan>(output_laser_topic.c_str(), 1);
                                        cout << "\t\t" << output_frames[i] << " on topic " << output_laser_topic.c_str() << endl;
                                }
                        }
                }
                else
                        ROS_INFO("Not publishing to any topic.");
        }
}

LaserscanVirtualizer::LaserscanVirtualizer()
{
        ros::NodeHandle nh("~");

        //Setting class parameters
        if (!nh.getParam("/laserscan_virtualizer/base_frame", base_frame))
                base_frame = "/cart_frame";

        if (!nh.getParam("/laserscan_virtualizer/cloud_topic", cloud_topic))
                cloud_topic = "/cloud_pcd";

        nh.getParam("/laserscan_virtualizer/output_laser_topic", output_laser_topic);
        nh.getParam("/laserscan_virtualizer/virtual_laser_scan", virtual_laser_scan);

        this->virtual_laser_scan_parser();

        point_cloud_subscriber_ = node_.subscribe<sensor_msgs::PointCloud2>(cloud_topic.c_str(), 1, boost::bind(&LaserscanVirtualizer::pointCloudCallback, this, _1));
        cloud_frame = "";
}

void LaserscanVirtualizer::pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &pcl_in)
{
        if (cloud_frame.empty())
        {
                cloud_frame = (*pcl_in).header.frame_id;
                transform_.resize(output_frames.size());
                for (int i = 0; i < output_frames.size(); i++)
                        if (tfListener_.waitForTransform(output_frames[i], cloud_frame, ros::Time(0), ros::Duration(2.0)))
                                tfListener_.lookupTransform(output_frames[i], cloud_frame, ros::Time(0), transform_[i]);
        }

        for (int i = 0; i < output_frames.size(); i++)
        {
                myPointCloud pcl_out, tmpPcl;
                pcl::PCLPointCloud2 tmpPcl2;

                pcl_conversions::toPCL(*pcl_in, tmpPcl2);
                pcl::fromPCLPointCloud2(tmpPcl2, tmpPcl);

                // Initialize the header of the temporary pointcloud, needed to rototranslate the three points that define our plane
                // It shall be equal to the input point cloud's one, changing only the 'frame_id'
                string tmpFrame = output_frames[i];
                pcl_out.header = tmpPcl.header;

                // Ask tf to rototranslate the velodyne pointcloud to base_frame reference
                pcl_ros::transformPointCloud(tmpPcl, pcl_out, transform_[i]);
                pcl_out.header.frame_id = tmpFrame;

                // Transform the pcl into eigen matrix
                Eigen::MatrixXf tmpEigenMatrix;
                pcl::toPCLPointCloud2(pcl_out, tmpPcl2);
                pcl::getPointCloudAsEigen(tmpPcl2, tmpEigenMatrix);

                // Extract the points close to the z=0 plane, convert them into a laser-scan message and publish it
                pointcloud_to_laserscan(tmpEigenMatrix, pcl_out.header, i);
        }
}

void LaserscanVirtualizer::pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLHeader scan_header, int pub_index) //pcl::PCLPointCloud2 *merged_cloud)
{
        sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
        output->header = pcl_conversions::fromPCL(scan_header);
        output->angle_min = this->angle_min;
        output->angle_max = this->angle_max;
        output->angle_increment = this->angle_increment;
        output->time_increment = this->time_increment;
        output->scan_time = this->scan_time;
        output->range_min = this->range_min;
        output->range_max = this->range_max;

        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);

        for (int i = 0; i < points.cols(); i++)
        {
                const float &x = points(0, i);
                const float &y = points(1, i);
                const float &z = points(2, i);

                if (std::isnan(x) || std::isnan(y) || std::isnan(z))
                {
                        ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
                        continue;
                }

                pcl::PointXYZ p;
                p.x = x;
                p.y = y;
                p.z = z;

                double range_sq = pow(y, 2) + pow(x, 2);
                double range_min_sq_ = output->range_min * output->range_min;
                if (range_sq < range_min_sq_)
                {
                        ROS_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z);
                        continue;
                }

                double angle = atan2(y, x);
                if (angle < output->angle_min || angle > output->angle_max)
                {
                        ROS_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;

                if (output->ranges[index] * output->ranges[index] > range_sq)
                        output->ranges[index] = sqrt(range_sq);
        }

        virtual_scan_publishers[pub_index].publish(output);
}

int main(int argc, char **argv)
{
        ros::init(argc, argv, "laserscan_virtualizer");

        LaserscanVirtualizer _laser_merger;

        dynamic_reconfigure::Server<laserscan_virtualizerConfig> server;
        dynamic_reconfigure::Server<laserscan_virtualizerConfig>::CallbackType f;

        f = boost::bind(&LaserscanVirtualizer::reconfigureCallback, &_laser_merger, _1, _2);
        server.setCallback(f);

        ros::spin();

        return 0;
}