output.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2014-2018, 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 #ifndef COSTMAP_CSPACE_COSTMAP_3D_LAYER_OUTPUT_H
00031 #define COSTMAP_CSPACE_COSTMAP_3D_LAYER_OUTPUT_H
00032 
00033 #include <geometry_msgs/PolygonStamped.h>
00034 #include <nav_msgs/OccupancyGrid.h>
00035 #include <costmap_cspace_msgs/CSpace3D.h>
00036 #include <costmap_cspace_msgs/CSpace3DUpdate.h>
00037 
00038 #include <costmap_cspace/costmap_3d_layer/base.h>
00039 
00040 namespace costmap_cspace
00041 {
00042 class Costmap3dLayerOutput : public Costmap3dLayerBase
00043 {
00044 public:
00045   using Ptr = std::shared_ptr<Costmap3dLayerOutput>;
00046   using Callback = boost::function<bool(const CSpace3DMsg::Ptr, const costmap_cspace_msgs::CSpace3DUpdate::Ptr)>;
00047 
00048 protected:
00049   Callback cb_;
00050   UpdatedRegion region_prev_;
00051 
00052 public:
00053   void loadConfig(XmlRpc::XmlRpcValue config)
00054   {
00055   }
00056   void setHandler(Callback cb)
00057   {
00058     cb_ = cb;
00059   }
00060   void setMapMetaData(const costmap_cspace_msgs::MapMetaData3D& info)
00061   {
00062   }
00063 
00064 protected:
00065   int getRangeMax() const
00066   {
00067     return 0;
00068   }
00069   bool updateChain(const bool output)
00070   {
00071     auto update_msg = generateUpdateMsg();
00072     if (cb_ && output)
00073       return cb_(map_, update_msg);
00074     return true;
00075   }
00076   void updateCSpace(
00077       const nav_msgs::OccupancyGrid::ConstPtr& map,
00078       const UpdatedRegion& region)
00079   {
00080   }
00081   costmap_cspace_msgs::CSpace3DUpdate::Ptr generateUpdateMsg()
00082   {
00083     costmap_cspace_msgs::CSpace3DUpdate::Ptr update_msg(new costmap_cspace_msgs::CSpace3DUpdate);
00084     update_msg->header = map_->header;
00085     map_->header.stamp = region_.stamp_;
00086     int update_x = region_.x_;
00087     int update_y = region_.y_;
00088     int update_width = region_.width_;
00089     int update_height = region_.height_;
00090     if (update_x < 0)
00091     {
00092       update_width += update_x;
00093       update_x = 0;
00094     }
00095     if (update_y < 0)
00096     {
00097       update_height += update_y;
00098       update_y = 0;
00099     }
00100     if (update_x + update_width > static_cast<int>(map_->info.width))
00101     {
00102       update_width = map_->info.width - update_x;
00103     }
00104     if (update_y + update_height > static_cast<int>(map_->info.height))
00105     {
00106       update_height = map_->info.height - update_y;
00107     }
00108     UpdatedRegion region(
00109         update_x,
00110         update_y,
00111         region_.yaw_,
00112         update_width,
00113         update_height,
00114         region_.angle_);
00115     UpdatedRegion region_merged = region;
00116     region_merged.merge(region_prev_);
00117     region_prev_ = region;
00118     region_ = UpdatedRegion();
00119 
00120     update_msg->x = region_merged.x_;
00121     update_msg->y = region_merged.y_;
00122     update_msg->width = region_merged.width_;
00123     update_msg->height = region_merged.height_;
00124     update_msg->yaw = region_merged.yaw_;
00125     update_msg->angle = region_merged.angle_;
00126     update_msg->data.resize(update_msg->width * update_msg->height * update_msg->angle);
00127 
00128     for (int i = 0; i < static_cast<int>(update_msg->width); i++)
00129     {
00130       for (int j = 0; j < static_cast<int>(update_msg->height); j++)
00131       {
00132         for (int k = 0; k < static_cast<int>(update_msg->angle); k++)
00133         {
00134           const int x2 = update_msg->x + i;
00135           const int y2 = update_msg->y + j;
00136           const int yaw2 = update_msg->yaw + k;
00137 
00138           const auto& m = map_->getCost(x2, y2, yaw2);
00139           const size_t addr = (k * update_msg->height + j) * update_msg->width + i;
00140           ROS_ASSERT(addr < update_msg->data.size());
00141           auto& up = update_msg->data[addr];
00142           up = m;
00143         }
00144       }
00145     }
00146     return update_msg;
00147   }
00148 };
00149 }  // namespace costmap_cspace
00150 
00151 #endif  // COSTMAP_CSPACE_COSTMAP_3D_LAYER_OUTPUT_H


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