pointcloud_model_generator_node.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 #include <ros/ros.h>
00036 #include "jsk_footstep_planner/pointcloud_model_generator.h"
00037 #include <jsk_footstep_planner/PointCloudModelGeneratorConfig.h>
00038 #include <sensor_msgs/PointCloud2.h>
00039 #include <jsk_recognition_utils/pcl_conversion_util.h>
00040 #include <dynamic_reconfigure/server.h>
00041 
00042 using namespace jsk_footstep_planner;
00043 
00044 double hole_rate;
00045 std::string model;
00046 boost::mutex mutex;
00047 void reconfigureCallback(PointCloudModelGeneratorConfig &config, uint32_t level)
00048 {
00049   boost::mutex::scoped_lock lock(mutex);
00050   hole_rate = config.hole_rate;
00051   model = config.model;
00052 }
00053 
00054 
00055 int main(int argc, char** argv)
00056 {
00057   ros::init(argc, argv, "pointcloud_model_generator_node");
00058   ros::NodeHandle pnh("~");
00059   ros::Rate r(1);
00060   ros::Publisher pub_cloud = pnh.advertise<sensor_msgs::PointCloud2>("output", 1, true);
00061   dynamic_reconfigure::Server<PointCloudModelGeneratorConfig> server;
00062   server.setCallback(boost::bind(&reconfigureCallback, _1, _2));
00063   while (ros::ok()) {
00064     ros::spinOnce();
00065     r.sleep();
00066     {
00067       boost::mutex::scoped_lock lock(mutex);
00068       PointCloudModelGenerator gen;
00069       pcl::PointCloud<pcl::PointNormal> cloud;
00070       gen.generate(model, cloud, hole_rate);
00071       sensor_msgs::PointCloud2 ros_cloud;
00072       pcl::toROSMsg(cloud, ros_cloud);
00073       ros_cloud.header.frame_id = "odom";
00074       ros_cloud.header.stamp = ros::Time::now();
00075       pub_cloud.publish(ros_cloud);
00076     }
00077   }
00078   return 0;
00079 }


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jul 19 2017 02:54:29