cob_gazebo: fix_gazebo_pointcloud.cpp Source File

00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_vision
00012  * ROS package name: cob_env_model
00013  * Description:
00014  *
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *
00017  * Author: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de
00018  * Supervised by: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de
00019  *
00020  * Date of creation: 05/2011
00021  * ToDo:
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Fraunhofer Institute for Manufacturing
00034  *       Engineering and Automation (IPA) nor the names of its
00035  *       contributors may be used to endorse or promote products derived from
00036  *       this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as
00040  * published by the Free Software Foundation, either version 3 of the
00041  * License, or (at your option) any later version.
00042  *
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  *
00048  * You should have received a copy of the GNU Lesser General Public
00049  * License LGPL along with this program.
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 #include <ros/ros.h>
00055 #include <sensor_msgs/PointCloud.h>
00056 
00057 
00058 class FixGazeboPointcloud
00059 {
00060 public:
00061         typedef sensor_msgs::PointCloud PointCloud;
00062 
00063         FixGazeboPointcloud()
00064         {
00065                 sub_ = nh_.subscribe("full_cloud", 1, &FixGazeboPointcloud::input_callback, this);
00066                 pub_ = nh_.advertise<sensor_msgs::PointCloud> ("full_cloud_fixed", 1);
00067         }
00068 
00069         void input_callback(const PointCloud::Ptr &cloud)
00070         {
00071                 //ROS_INFO("[ConcatNormals] SyncCallback");
00072                 //ROS_INFO("channels: %d", cloud->channels.size());
00073                 cloud->channels.clear();
00074                 //ROS_INFO("channels: %d", cloud->channels.size());
00075                 //cloud->channels[0].name = std::string("unknown");
00076 
00077                 pub_.publish (cloud);
00078 
00079         }
00080 
00081         ros::NodeHandle nh_;
00082         ros::Subscriber sub_;
00083         ros::Publisher pub_;
00084 
00085 
00086 };
00087 
00088 int main(int argc, char** argv)
00089 {
00090         ros::init(argc, argv, "fix_gazebo_pointcloud_node");
00091         FixGazeboPointcloud fix;
00092 
00093         ros::spin();
00094 
00095         return 0;
00096 }