Guest User

cloud_assembler.cpp

a guest
Feb 17th, 2012
753
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*
  2.  * cloud_assembler.cpp
  3.  *
  4.  *  Created on: Nov 11, 2011
  5.  *      Author: dimitri prosser
  6.  */
  7.  
  8. #include <ros/ros.h>
  9. #include <sensor_msgs/PointCloud2.h>
  10. #include <pcl/ros/conversions.h>
  11. #include <pcl/point_types.h>
  12. #include <tf/transform_listener.h>
  13. #include <Eigen/Core>
  14. #include <boost/algorithm/string.hpp>
  15. #include <std_srvs/Empty.h>
  16. #include <tf/transform_listener.h>
  17.  
  18. using namespace pcl;
  19.  
  20. namespace cloud_assembler
  21. {
  22.  
  23. typedef PointCloud<PointXYZ> PointCloud2;
  24.  
  25. class CloudAssembler
  26. {
  27.  
  28. public:
  29.   CloudAssembler();
  30.   void cloudCallback(const sensor_msgs::PointCloud2& cloud);
  31.  
  32. private:
  33.   ros::NodeHandle node_;
  34.  
  35.   ros::ServiceServer pause_srv_;
  36.  
  37.   ros::Publisher output_pub_;
  38.   ros::Subscriber cloud_sub_;
  39.  
  40.   tf::TransformListener tf_;
  41.  
  42.   PointCloud2 assembled_cloud_;
  43.   int buffer_length_;
  44.   std::vector<sensor_msgs::PointCloud2> cloud_buffer_;
  45.   bool assemblerPaused_;
  46.  
  47.   void addToBuffer(sensor_msgs::PointCloud2 cloud);
  48.   void assembleCloud();
  49.   bool pauseSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp);
  50.  
  51. };
  52.  
  53. CloudAssembler::CloudAssembler()
  54. {
  55.   ros::NodeHandle private_nh("~");
  56.  
  57.   private_nh.param("buffer_length", buffer_length_, 50);
  58.  
  59.   output_pub_ = node_.advertise<sensor_msgs::PointCloud2> ("/assembled_cloud", 100);
  60.  
  61.   pause_srv_ = node_.advertiseService("/pause_assembler", &CloudAssembler::pauseSrv, this);
  62.  
  63.   cloud_sub_ = node_.subscribe("/input_cloud", 100, &CloudAssembler::cloudCallback, this);
  64.  
  65.   PointCloud2 clear;
  66.   assembled_cloud_ = clear;
  67.  
  68.   assemblerPaused_ = false;
  69. }
  70.  
  71. void CloudAssembler::cloudCallback(const sensor_msgs::PointCloud2& cloud)
  72. {
  73.   addToBuffer(cloud);
  74.   assembleCloud();
  75.  
  76.   sensor_msgs::PointCloud2 cloud_msg;
  77.   pcl::toROSMsg(assembled_cloud_, cloud_msg);
  78.  
  79.   cloud_msg.header.frame_id = cloud.header.frame_id;
  80.   cloud_msg.header.stamp = ros::Time::now();
  81.  
  82.   output_pub_.publish(cloud_msg);
  83. }
  84.  
  85. void CloudAssembler::assembleCloud()
  86. {
  87.   ROS_DEBUG("Assembling.");
  88.  
  89.   unsigned int i;
  90.  
  91.   if (assemblerPaused_)
  92.   {
  93.     ROS_INFO("assemblerPaused_ is true");
  94.   }
  95.   if (!assemblerPaused_)
  96.   {
  97.     ROS_DEBUG("assemblerPaused_ is false");
  98.   }
  99.  
  100.   std::string fixed_frame = cloud_buffer_[0].header.frame_id;
  101.  
  102.   PointCloud2 new_cloud;
  103.   new_cloud.header.frame_id = fixed_frame;
  104.   new_cloud.header.stamp = ros::Time::now();
  105.  
  106.   for (i = 0; i < cloud_buffer_.size(); i++)
  107.   {
  108.     PointCloud2 temp_cloud;
  109.     pcl::fromROSMsg(cloud_buffer_[i], temp_cloud);
  110.     temp_cloud.header.frame_id = fixed_frame;
  111.     new_cloud += temp_cloud;
  112.   }
  113.  
  114.   // If it's paused, don't overwrite the stored cloud with a new one, just keep publishing the same cloud
  115.   if (!assemblerPaused_)
  116.   {
  117.     assembled_cloud_ = new_cloud;
  118.   }
  119.   else if (assemblerPaused_)
  120.   {
  121.     ROS_DEBUG("The Assembler will continue to publish the same cloud.");
  122.   }
  123.  
  124. }
  125.  
  126. bool CloudAssembler::pauseSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp)
  127. {
  128.   ROS_DEBUG("In service call: %s", assemblerPaused_?"true":"false");
  129.  
  130.   if (!assemblerPaused_)
  131.   {
  132.     ROS_DEBUG("Now paused.");
  133.     assemblerPaused_ = true;
  134.   }
  135.   else if (assemblerPaused_)
  136.   {
  137.     assemblerPaused_ = false;
  138.     ROS_DEBUG("Unpaused.");
  139.   }
  140.  
  141.   return true;
  142. }
  143.  
  144. void CloudAssembler::addToBuffer(sensor_msgs::PointCloud2 cloud)
  145. {
  146.   ROS_DEBUG("Adding cloud to buffer. Current buffer length is %d", cloud_buffer_.size());
  147.  
  148.   if (cloud_buffer_.size() >= (unsigned int)buffer_length_)
  149.   {
  150.     cloud_buffer_.erase(cloud_buffer_.begin());
  151.   }
  152.  
  153.   cloud_buffer_.push_back(cloud);
  154. }
  155.  
  156.  
  157. }; // namespace
  158.  
  159. int main(int argc, char ** argv)
  160. {
  161.   ros::init(argc, argv, "cloud_assembler");
  162.   cloud_assembler::CloudAssembler cloud_assembler;
  163.  
  164.   ros::spin();
  165.  
  166.   return 0;
  167. }
RAW Paste Data