Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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 }
00150
00151 #endif // COSTMAP_CSPACE_COSTMAP_3D_LAYER_OUTPUT_H