test_rosmpi.cpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: test_rosmpi.cpp 48321 2011-01-26 07:05:47Z hsu $
 *
 */

#include <rosmpi/rosmpi.h>
#include <rosmpi/node_handle.h>
#include <rosmpi/publisher.h>
#include <rosmpi/subscriber.h>

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/ros/conversions.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>

typedef sensor_msgs::PointCloud2 Msg;
typedef sensor_msgs::PointCloud2::ConstPtr MsgConstPtr;

std::string processor_name;
int rank;

void 
  computeIndices (const int rank, const int max_rank, const int nr_points, pcl::PointIndices &indices)
{
  int pts_per_worker = lrint (round (nr_points / max_rank));
  int s_idx = rank * pts_per_worker;
  //int e_idx = (rank + 1) * pts_per_worker;
  indices.indices.resize (pts_per_worker);
  for (size_t i = 0; i < indices.indices.size (); i++)
    indices.indices[i] = i + s_idx;
}

class Worker
{
  public:
    Worker (int rank, int max_rank) : rank_ (rank), max_rank_ (max_rank)
    {
      sub_ = nh_.subscribe (&Worker::cloud_cb, this);
    }


    void 
      cloud_cb (const MsgConstPtr &msg)
    {
      ROS_INFO_STREAM ("[" << processor_name << "/" << rank << "] received message with " << msg->data.size () << " size, with " << msg->width << "x" << msg->height);

      pcl::PointCloud<pcl::PointXYZ> cloud;
      pcl::fromROSMsg (*msg, cloud);
      cloud_.reset (new pcl::PointCloud<pcl::PointXYZ> (cloud));
     
      pcl::PointIndices indices;
      computeIndices (rank_, max_rank_, cloud.points.size (), indices);

      tree_ = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
      tree_->setInputCloud (cloud_);
      
      ROS_INFO ("Estimating features for indices: %d -> %d", indices.indices[0], indices.indices[indices.indices.size () - 1]);
      ne_.setInputCloud (cloud_);
      ne_.setIndices (boost::make_shared<pcl::PointIndices> (indices));
      ne_.setSearchMethod (tree_);
      ne_.setRadiusSearch (0.03);
      ne_.compute (normals_);
    }

  private:
    rosmpi::NodeHandle nh_;
    rosmpi::Subscriber sub_;
    pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree_;
    int rank_, max_rank_;

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_;
    pcl::PointCloud<pcl::Normal> normals_;
};

/* ---[ */
int
  main (int argc, char** argv)
{
  if (argc < 2)
  {
    ROS_ERROR ("Need a valid PCD file as input. Example: $ %s `rospack find pcl`/test/bun0.pcd", argv[0]);
    return (-1);
  }
  rosmpi::init (argc, argv, rosmpi::init_options::ThreadSingle);      // Init ros-mpi

  // Get the process name and rank
  int nr_procs = rosmpi::communicator::getSize ();
  rank     = rosmpi::communicator::getRank ();

  processor_name = rosmpi::this_processor::getName ();
 
  // Master?
  if (rank == 0)
  {
    // Load the data
    Msg msg;
    pcl::io::loadPCDFile (argv[1], msg);
    ROS_INFO_STREAM ("[" << processor_name << "] Loaded a file with " << msg.width * msg.height << " points (" << msg.data.size () << " data bytes).");

    // Create the publisher
    rosmpi::Publisher pub; 
    ROS_INFO_STREAM ("[" << processor_name << "] Sending data to " << nr_procs << " nodes.") ;
    pub.publish<Msg> (msg);

      pcl::PointCloud<pcl::PointXYZ> cloud;
      pcl::fromROSMsg (msg, cloud);
     
      pcl::PointIndices indices;
      computeIndices (rank, nr_procs, cloud.points.size (), indices);

      pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree_ = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
      tree_->setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud));
      
      ROS_INFO ("Estimating features for indices: %d -> %d", indices.indices[0], indices.indices[indices.indices.size () - 1]);
      pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_;
      ne_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud));
      ne_.setIndices (boost::make_shared<pcl::PointIndices> (indices));
      ne_.setSearchMethod (tree_);
      ne_.setRadiusSearch (0.03);
      pcl::PointCloud<pcl::Normal> normals_;
      ne_.compute (normals_);
  }
  else
  {
    Worker w (rank, nr_procs);
  }

  rosmpi::shutdown ();

  return (0);
}
/* ]--- */