00001 #include <ros/ros.h> 00002 00003 #include <hrl_phri_2011/pcl_basic.h> 00004 #include <hrl_phri_2011/utils.h> 00005 #include <hrl_phri_2011/ForceProcessed.h> 00006 #include <hrl_phri_2011/hsl_rgb_conversions.h> 00007 00008 int main(int argc, char **argv) 00009 { 00010 ros::init(argc, argv, "concat_clouds"); 00011 ros::NodeHandle nh; 00012 00013 if(argc == 1) { 00014 printf("Usage: concat_clouds topic cloud_bag1 ... output_bag\n"); 00015 return 1; 00016 } 00017 00018 PCRGB out_pc; 00019 for(int i=2;i<argc-1;i++) { 00020 vector<PCRGB::Ptr> cloud; 00021 readBagTopic<PCRGB>(argv[i], cloud, argv[1]); 00022 if(out_pc.size() != 0) 00023 out_pc += *cloud[0]; 00024 else 00025 out_pc = *cloud[0]; 00026 } 00027 00028 rosbag::Bag bag; 00029 bag.open(argv[argc-1], rosbag::bagmode::Write); 00030 bag.write("/contact_cloud", ros::Time::now(), out_pc); 00031 bag.close(); 00032 00033 return 0; 00034 }