pointcloud2_to_map.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2014-2017, the neonavigation authors
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the copyright holder nor the names of its 
00014  *       contributors may be used to endorse or promote products derived from 
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 
00032 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00033 #include <tf2_ros/transform_listener.h>
00034 #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
00035 #include <nav_msgs/OccupancyGrid.h>
00036 #include <sensor_msgs/PointCloud2.h>
00037 
00038 #include <string>
00039 #include <vector>
00040 
00041 #include <costmap_cspace/pointcloud_accumulator/accumulator.h>
00042 #include <neonavigation_common/compatibility.h>
00043 
00044 class Pointcloud2ToMapNode
00045 {
00046 private:
00047   ros::NodeHandle nh_;
00048   ros::NodeHandle pnh_;
00049   ros::Publisher pub_map_;
00050   ros::Subscriber sub_cloud_;
00051   ros::Subscriber sub_cloud_single_;
00052 
00053   nav_msgs::OccupancyGrid map_;
00054   tf2_ros::Buffer tfbuf_;
00055   tf2_ros::TransformListener tfl_;
00056   ros::Time published_;
00057   ros::Duration publish_interval_;
00058 
00059   double z_min_, z_max_;
00060   std::string global_frame_;
00061   std::string robot_frame_;
00062 
00063   unsigned int width_;
00064   unsigned int height_;
00065   float origin_x_;
00066   float origin_y_;
00067 
00068   std::vector<PointcloudAccumurator<sensor_msgs::PointCloud2>> accums_;
00069 
00070 public:
00071   Pointcloud2ToMapNode()
00072     : nh_()
00073     , pnh_("~")
00074     , tfl_(tfbuf_)
00075     , accums_(2)
00076   {
00077     neonavigation_common::compat::checkCompatMode();
00078     pnh_.param("z_min", z_min_, 0.1);
00079     pnh_.param("z_max", z_max_, 1.0);
00080     pnh_.param("global_frame", global_frame_, std::string("map"));
00081     pnh_.param("robot_frame", robot_frame_, std::string("base_link"));
00082 
00083     double accum_duration;
00084     pnh_.param("accum_duration", accum_duration, 1.0);
00085     accums_[0].reset(ros::Duration(accum_duration));
00086     accums_[1].reset(ros::Duration(0.0));
00087 
00088     pub_map_ = neonavigation_common::compat::advertise<nav_msgs::OccupancyGrid>(
00089         nh_, "map_local",
00090         pnh_, "map", 1, true);
00091     sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>(
00092         "cloud", 100,
00093         boost::bind(&Pointcloud2ToMapNode::cbCloud, this, _1, false));
00094     sub_cloud_single_ = nh_.subscribe<sensor_msgs::PointCloud2>(
00095         "cloud_singleshot", 100,
00096         boost::bind(&Pointcloud2ToMapNode::cbCloud, this, _1, true));
00097 
00098     int width_param;
00099     pnh_.param("width", width_param, 30);
00100     height_ = width_ = width_param;
00101     map_.header.frame_id = global_frame_;
00102 
00103     double resolution;
00104     pnh_.param("resolution", resolution, 0.1);
00105     map_.info.resolution = resolution;
00106     map_.info.width = width_;
00107     map_.info.height = height_;
00108     map_.data.resize(map_.info.width * map_.info.height);
00109 
00110     double hz;
00111     pnh_.param("hz", hz, 1.0);
00112     publish_interval_ = ros::Duration(1.0 / hz);
00113   }
00114 
00115 private:
00116   void cbCloud(const sensor_msgs::PointCloud2::ConstPtr& cloud, const bool singleshot)
00117   {
00118     sensor_msgs::PointCloud2 cloud_global;
00119     geometry_msgs::TransformStamped trans;
00120     try
00121     {
00122       trans = tfbuf_.lookupTransform(global_frame_, cloud->header.frame_id,
00123                                      cloud->header.stamp, ros::Duration(0.5));
00124     }
00125     catch (tf2::TransformException& e)
00126     {
00127       ROS_WARN("%s", e.what());
00128       return;
00129     }
00130     tf2::doTransform(*cloud, cloud_global, trans);
00131 
00132     const int buffer = singleshot ? 1 : 0;
00133     accums_[buffer].push(PointcloudAccumurator<sensor_msgs::PointCloud2>::Points(
00134         cloud_global, cloud_global.header.stamp));
00135 
00136     ros::Time now = cloud->header.stamp;
00137     if (published_ + publish_interval_ > now)
00138       return;
00139     published_ = now;
00140 
00141     float robot_z;
00142     try
00143     {
00144       tf2::Stamped<tf2::Transform> trans;
00145       tf2::fromMsg(tfbuf_.lookupTransform(global_frame_, robot_frame_, ros::Time(0)), trans);
00146 
00147       auto pos = trans.getOrigin();
00148       float x = static_cast<int>(pos.x() / map_.info.resolution) * map_.info.resolution;
00149       float y = static_cast<int>(pos.y() / map_.info.resolution) * map_.info.resolution;
00150       map_.info.origin.position.x = x - map_.info.width * map_.info.resolution * 0.5;
00151       map_.info.origin.position.y = y - map_.info.height * map_.info.resolution * 0.5;
00152       map_.info.origin.position.z = 0.0;
00153       map_.info.origin.orientation.w = 1.0;
00154       origin_x_ = x - width_ * map_.info.resolution * 0.5;
00155       origin_y_ = y - height_ * map_.info.resolution * 0.5;
00156       robot_z = pos.z();
00157     }
00158     catch (tf2::TransformException& e)
00159     {
00160       ROS_WARN("%s", e.what());
00161       return;
00162     }
00163     for (auto& cell : map_.data)
00164       cell = 0;
00165 
00166     for (auto& accum : accums_)
00167     {
00168       for (auto& pc : accum)
00169       {
00170         sensor_msgs::PointCloud2Iterator<float> iter_x(pc, "x");
00171         sensor_msgs::PointCloud2Iterator<float> iter_y(pc, "y");
00172         sensor_msgs::PointCloud2Iterator<float> iter_z(pc, "z");
00173         for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
00174         {
00175           if (*iter_z - robot_z < z_min_ || z_max_ < *iter_z - robot_z)
00176             continue;
00177           unsigned int x = int(
00178               (*iter_x - map_.info.origin.position.x) / map_.info.resolution);
00179           unsigned int y = int(
00180               (*iter_y - map_.info.origin.position.y) / map_.info.resolution);
00181           if (x >= map_.info.width || y >= map_.info.height)
00182             continue;
00183           map_.data[x + y * map_.info.width] = 100;
00184         }
00185       }
00186     }
00187 
00188     pub_map_.publish(map_);
00189   }
00190 };
00191 
00192 int main(int argc, char** argv)
00193 {
00194   ros::init(argc, argv, "pointcloud2_to_map");
00195 
00196   Pointcloud2ToMapNode conv;
00197   ros::spin();
00198 
00199   return 0;
00200 }


costmap_cspace
Author(s): Atsushi Watanabe
autogenerated on Sat Jun 22 2019 20:07:13