diff --git a/costmap_cspace/include/costmap_3d_layer_base.h b/costmap_cspace/include/costmap_3d_layer_base.h index 6765813e..2a15f94b 100644 --- a/costmap_cspace/include/costmap_3d_layer_base.h +++ b/costmap_cspace/include/costmap_3d_layer_base.h @@ -30,6 +30,8 @@ #ifndef COSTMAP_3D_LAYER_BASE_H #define COSTMAP_3D_LAYER_BASE_H +#include + #include #include #include @@ -39,6 +41,33 @@ namespace costmap_cspace { +class CSpace3DMsg : public CSpace3D +{ +public: + using Ptr = std::shared_ptr; + const int8_t &getCost(const int &x, const int &y, const int &yaw) const + { + ROS_ASSERT(static_cast(yaw) < info.angle); + ROS_ASSERT(static_cast(x) < info.width); + ROS_ASSERT(static_cast(y) < info.height); + + const size_t addr = (yaw * info.height + y) * info.width + x; + ROS_ASSERT(addr < data.size()); + + return data[addr]; + } + int8_t &getCost(const int &x, const int &y, const int &yaw) + { + ROS_ASSERT(static_cast(yaw) < info.angle); + ROS_ASSERT(static_cast(x) < info.width); + ROS_ASSERT(static_cast(y) < info.height); + + const size_t addr = (yaw * info.height + y) * info.width + x; + ROS_ASSERT(addr < data.size()); + + return data[addr]; + } +}; class Costmap3dLayerBase { public: @@ -56,11 +85,11 @@ class Costmap3dLayerBase float linear_spread_; map_overlay_mode overlay_mode_; - costmap_cspace::CSpace3Cache cs_; + CSpace3Cache cs_template_; int range_max_; - costmap_cspace::CSpace3D::Ptr map_; - costmap_cspace::CSpace3D::Ptr map_overlay_; + CSpace3DMsg::Ptr map_; + CSpace3DMsg::Ptr map_overlay_; nav_msgs::OccupancyGrid map_base_; Costmap3dLayerBase::Ptr child_; @@ -71,8 +100,8 @@ class Costmap3dLayerBase , linear_expand_(0.0) , linear_spread_(0.0) , overlay_mode_(map_overlay_mode::MAX) - , map_(new costmap_cspace::CSpace3D) - , map_overlay_(new costmap_cspace::CSpace3D) + , map_(new CSpace3DMsg) + , map_overlay_(new CSpace3DMsg) { } void setCSpaceConfig( @@ -86,10 +115,10 @@ class Costmap3dLayerBase linear_spread_ = linear_spread; overlay_mode_ = overlay_mode; } - virtual void generateCSpaceTemplate(const costmap_cspace::MapMetaData3D &info) = 0; + virtual void generateCSpaceTemplate(const MapMetaData3D &info) = 0; void setBaseMap(const nav_msgs::OccupancyGrid &base_map) { - assert(base_map.data.size() >= base_map.info.width * base_map.info.height); + ROS_ASSERT(base_map.data.size() >= base_map.info.width * base_map.info.height); map_base_ = base_map; map_->header = map_base_.header; @@ -130,7 +159,7 @@ class Costmap3dLayerBase child_ = child; child_->setMap(getMapOverlay()); } - costmap_cspace::CSpace3DUpdate processMapOverlay(const nav_msgs::OccupancyGrid &msg) + CSpace3DUpdate processMapOverlay(const nav_msgs::OccupancyGrid &msg) { const int ox = lroundf((msg.info.origin.position.x - map_->info.origin.position.x) / map_->info.linear_resolution); const int oy = lroundf((msg.info.origin.position.y - map_->info.origin.position.y) / map_->info.linear_resolution); @@ -141,21 +170,21 @@ class Costmap3dLayerBase const int h = lroundf(msg.info.height * msg.info.resolution / map_->info.linear_resolution); return generateUpdateMsg(map_overlay_, ox, oy, 0, w, h, map_->info.angle, msg.header.stamp); } - costmap_cspace::CSpace3D::Ptr getMap() + CSpace3DMsg::Ptr getMap() { return map_; } - void setMap(costmap_cspace::CSpace3D::Ptr map) + void setMap(CSpace3DMsg::Ptr map) { map_ = map; } - costmap_cspace::CSpace3D::Ptr getMapOverlay() + CSpace3DMsg::Ptr getMapOverlay() { return map_overlay_; } CSpace3Cache &getTemplate() { - return cs_; + return cs_template_; } int getRangeMax() const { @@ -165,36 +194,6 @@ class Costmap3dLayerBase { return ang_grid_; } - int8_t getCost(const int &x, const int &y, const int &yaw) const - { - return getCost(x, y, yaw, map_overlay_); - } - int8_t getCost(const int &x, const int &y, const int &yaw, costmap_cspace::CSpace3D::ConstPtr map) const - { - assert(static_cast(yaw) < map->info.angle); - assert(static_cast(x) < map->info.width); - assert(static_cast(y) < map->info.height); - - const size_t addr = (yaw * map->info.height + y) * map->info.width + x; - assert(addr < map->data.size()); - - return map->data[addr]; - } - int8_t &getCostRef(const int &x, const int &y, const int &yaw) - { - return getCostRef(x, y, yaw, map_overlay_); - } - int8_t &getCostRef(const int &x, const int &y, const int &yaw, costmap_cspace::CSpace3D::Ptr map) - { - assert(static_cast(yaw) < map->info.angle); - assert(static_cast(x) < map->info.width); - assert(static_cast(y) < map->info.height); - - const size_t addr = (yaw * map->info.height + y) * map->info.width + x; - assert(addr < map->data.size()); - - return map->data[addr]; - } protected: void setBaseMapChain() @@ -204,7 +203,7 @@ class Costmap3dLayerBase if (child_) child_->setBaseMapChain(); } - void gemerateCSpace(costmap_cspace::CSpace3D::Ptr map, const nav_msgs::OccupancyGrid &msg, bool overlay = false) + void gemerateCSpace(CSpace3DMsg::Ptr map, const nav_msgs::OccupancyGrid &msg, bool overlay = false) { const int ox = lroundf((msg.info.origin.position.x - map->info.origin.position.x) / map->info.linear_resolution); const int oy = lroundf((msg.info.origin.position.y - map->info.origin.position.y) / map->info.linear_resolution); @@ -237,7 +236,7 @@ class Costmap3dLayerBase static_cast(y2) >= map->info.height) continue; - auto &m = getCostRef(x2, y2, yaw, map); + auto &m = map->getCost(x2, y2, yaw); m = 0; } } @@ -269,8 +268,8 @@ class Costmap3dLayerBase static_cast(y2) >= map->info.height) continue; - auto &m = getCostRef(x2, y2, yaw, map); - const auto c = cs_.e(x, y, yaw) * val / 100; + auto &m = map->getCost(x2, y2, yaw); + const auto c = cs_template_.e(x, y, yaw) * val / 100; if (m < c) m = c; } @@ -278,12 +277,12 @@ class Costmap3dLayerBase } } } - costmap_cspace::CSpace3DUpdate generateUpdateMsg( - costmap_cspace::CSpace3D::Ptr map, const int &x, const int &y, const int &yaw, + CSpace3DUpdate generateUpdateMsg( + CSpace3DMsg::Ptr map, const int &x, const int &y, const int &yaw, const int &width, const int &height, const int &angle, const ros::Time &stamp) { - costmap_cspace::CSpace3DUpdate update; + CSpace3DUpdate update; update.header = map->header; map->header.stamp = stamp; int update_x = x - range_max_; @@ -326,9 +325,9 @@ class Costmap3dLayerBase const int y2 = update.y + j; const int yaw2 = yaw + k; - const auto &m = getCostRef(x2, y2, yaw2, map); + const auto &m = map->getCost(x2, y2, yaw2); const size_t addr = (k * update.height + j) * update.width + i; - assert(addr < update.data.size()); + ROS_ASSERT(addr < update.data.size()); auto &up = update.data[addr]; up = m; } diff --git a/costmap_cspace/include/costmap_3d_layer_footprint.h b/costmap_cspace/include/costmap_3d_layer_footprint.h index ea3425ad..d1892a1b 100644 --- a/costmap_cspace/include/costmap_3d_layer_footprint.h +++ b/costmap_cspace/include/costmap_3d_layer_footprint.h @@ -30,6 +30,8 @@ #ifndef COSTMAP_3D_LAYER_FOOTPRINT_H #define COSTMAP_3D_LAYER_FOOTPRINT_H +#include + #include #include #include @@ -59,7 +61,7 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase protected: float footprint_radius_; geometry_msgs::PolygonStamped footprint_; - costmap_cspace::Polygon footprint_p_; + Polygon footprint_p_; public: Costmap3dLayerFootprint() @@ -85,7 +87,7 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase } geometry_msgs::Point32 point; - costmap_cspace::Vec v; + Vec v; v[0] = point.x = static_cast(footprint_xml[i][0]); v[1] = point.y = static_cast(footprint_xml[i][1]); point.z = 0; @@ -102,13 +104,13 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase return true; } - void generateCSpaceTemplate(const costmap_cspace::MapMetaData3D &info) + void generateCSpaceTemplate(const MapMetaData3D &info) { - assert(footprint_p_.v.size() > 2); + ROS_ASSERT(footprint_p_.v.size() > 2); range_max_ = ceilf((footprint_radius_ + linear_expand_ + linear_spread_) / info.linear_resolution); - cs_.reset(range_max_, range_max_, info.angle); + cs_template_.reset(range_max_, range_max_, info.angle); // C-Space template for (size_t yaw = 0; yaw < info.angle; yaw++) @@ -121,27 +123,27 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase f.move(x * info.linear_resolution, y * info.linear_resolution, yaw * info.angular_resolution); - costmap_cspace::Vec p; + Vec p; p[0] = 0; p[1] = 0; if (f.inside(p)) { - cs_.e(x, y, yaw) = 100; + cs_template_.e(x, y, yaw) = 100; } else { const float d = f.dist(p); if (d < linear_expand_) { - cs_.e(x, y, yaw) = 100; + cs_template_.e(x, y, yaw) = 100; } else if (d < linear_expand_ + linear_spread_) { - cs_.e(x, y, yaw) = 100 - (d - linear_expand_) * 100 / linear_spread_; + cs_template_.e(x, y, yaw) = 100 - (d - linear_expand_) * 100 / linear_spread_; } else { - cs_.e(x, y, yaw) = 0; + cs_template_.e(x, y, yaw) = 0; } } } diff --git a/costmap_cspace/include/costmap_3d_layer_plain.h b/costmap_cspace/include/costmap_3d_layer_plain.h index 202b2321..086ca61e 100644 --- a/costmap_cspace/include/costmap_3d_layer_plain.h +++ b/costmap_cspace/include/costmap_3d_layer_plain.h @@ -48,10 +48,10 @@ class Costmap3dLayerPlain : public Costmap3dLayerBase Costmap3dLayerPlain() { } - void generateCSpaceTemplate(const costmap_cspace::MapMetaData3D &info) + void generateCSpaceTemplate(const MapMetaData3D &info) { range_max_ = ceilf((linear_expand_ + linear_spread_) / info.linear_resolution); - cs_.reset(range_max_, range_max_, info.angle); + cs_template_.reset(range_max_, range_max_, info.angle); // C-Space template for (size_t yaw = 0; yaw < info.angle; yaw++) @@ -62,22 +62,22 @@ class Costmap3dLayerPlain : public Costmap3dLayerBase { if (x == 0 && y == 0) { - cs_.e(x, y, yaw) = 100; + cs_template_.e(x, y, yaw) = 100; } else { const float d = hypotf(x, y); if (d < linear_expand_) { - cs_.e(x, y, yaw) = 100; + cs_template_.e(x, y, yaw) = 100; } else if (d < linear_expand_ + linear_spread_) { - cs_.e(x, y, yaw) = 100 - (d - linear_expand_) * 100 / linear_spread_; + cs_template_.e(x, y, yaw) = 100 - (d - linear_expand_) * 100 / linear_spread_; } else { - cs_.e(x, y, yaw) = 0; + cs_template_.e(x, y, yaw) = 0; } } } diff --git a/costmap_cspace/include/cspace3_cache.h b/costmap_cspace/include/cspace3_cache.h index 9a642b2e..8203e5a9 100644 --- a/costmap_cspace/include/cspace3_cache.h +++ b/costmap_cspace/include/cspace3_cache.h @@ -30,6 +30,8 @@ #ifndef CSPACE3_CACHE_H #define CSPACE3_CACHE_H +#include + namespace costmap_cspace { class CSpace3Cache @@ -53,17 +55,17 @@ class CSpace3Cache char &e(const int &x, const int &y, const int &yaw) { - assert(-center[0] <= x && x < size[0] - center[0]); - assert(-center[1] <= y && y < size[1] - center[1]); - assert(-center[2] <= yaw && yaw < size[2] - center[2]); + ROS_ASSERT(-center[0] <= x && x < size[0] - center[0]); + ROS_ASSERT(-center[1] <= y && y < size[1] - center[1]); + ROS_ASSERT(-center[2] <= yaw && yaw < size[2] - center[2]); return c[(yaw * size[1] + (y + center[1])) * size[0] + x + center[0]]; } const char &e(const int &x, const int &y, const int &yaw) const { - assert(-center[0] <= x && x < size[0] - center[0]); - assert(-center[1] <= y && y < size[1] - center[1]); - assert(-center[2] <= yaw && yaw < size[2] - center[2]); + ROS_ASSERT(-center[0] <= x && x < size[0] - center[0]); + ROS_ASSERT(-center[1] <= y && y < size[1] - center[1]); + ROS_ASSERT(-center[2] <= yaw && yaw < size[2] - center[2]); return c[(yaw * size[1] + (y + center[1])) * size[0] + x + center[0]]; } diff --git a/costmap_cspace/include/polygon.h b/costmap_cspace/include/polygon.h index 739eb916..7254a6ea 100644 --- a/costmap_cspace/include/polygon.h +++ b/costmap_cspace/include/polygon.h @@ -30,6 +30,7 @@ #ifndef POLYGON_H #define POLYGON_H +#include #include namespace costmap_cspace @@ -40,12 +41,12 @@ class Vec float c[2]; float &operator[](const int &i) { - assert(i < 2); + ROS_ASSERT(i < 2); return c[i]; } const float &operator[](const int &i) const { - assert(i < 2); + ROS_ASSERT(i < 2); return c[i]; } Vec operator-(const Vec &a) const diff --git a/costmap_cspace/src/costmap_3d.cpp b/costmap_cspace/src/costmap_3d.cpp index 44083d63..246a5d45 100644 --- a/costmap_cspace/src/costmap_3d.cpp +++ b/costmap_cspace/src/costmap_3d.cpp @@ -69,8 +69,8 @@ class Costmap3DOFNode ROS_DEBUG("C-Space costmap generated"); auto map = costmaps_[0]->getMap(); - pub_costmap_.publish(*map); - publishDebug(map); + pub_costmap_.publish(*map); + publishDebug(*map); } void cbMapOverlay(const nav_msgs::OccupancyGrid::ConstPtr &msg) { @@ -89,25 +89,25 @@ class Costmap3DOFNode costmap_cspace::CSpace3DUpdate update = costmaps_[1]->processMapOverlay(*msg); ROS_DEBUG("C-Space costmap updated"); - publishDebug(map_overlay); + publishDebug(*map_overlay); pub_costmap_update_.publish(update); } - void publishDebug(costmap_cspace::CSpace3D::Ptr map) + void publishDebug(const costmap_cspace::CSpace3D &map) { sensor_msgs::PointCloud pc; - pc.header = map->header; + pc.header = map.header; pc.header.stamp = ros::Time::now(); - for (size_t yaw = 0; yaw < map->info.angle; yaw++) + for (size_t yaw = 0; yaw < map.info.angle; yaw++) { - for (unsigned int i = 0; i < map->info.width * map->info.height; i++) + for (unsigned int i = 0; i < map.info.width * map.info.height; i++) { - int gx = i % map->info.width; - int gy = i / map->info.width; - if (map->data[i + yaw * map->info.width * map->info.height] < 100) + int gx = i % map.info.width; + int gy = i / map.info.width; + if (map.data[i + yaw * map.info.width * map.info.height] < 100) continue; geometry_msgs::Point32 p; - p.x = gx * map->info.linear_resolution + map->info.origin.position.x; - p.y = gy * map->info.linear_resolution + map->info.origin.position.y; + p.x = gx * map.info.linear_resolution + map.info.origin.position.x; + p.y = gy * map.info.linear_resolution + map.info.origin.position.y; p.z = yaw * 0.1; pc.points.push_back(p); } @@ -122,10 +122,6 @@ class Costmap3DOFNode } public: - void spin() - { - ros::spin(); - } Costmap3DOFNode() : nh_() , nhp_("~") @@ -203,7 +199,7 @@ int main(int argc, char *argv[]) ros::init(argc, argv, "costmap_3d"); Costmap3DOFNode cm; - cm.spin(); + ros::spin(); return 0; } diff --git a/costmap_cspace/test/src/test_costmap_3d.cpp b/costmap_cspace/test/src/test_costmap_3d.cpp index 0216c89b..6c6755ed 100644 --- a/costmap_cspace/test/src/test_costmap_3d.cpp +++ b/costmap_cspace/test/src/test_costmap_3d.cpp @@ -33,6 +33,7 @@ #include +#include #include #include @@ -214,7 +215,7 @@ TEST(Costmap3dLayerFootprintTest, testCSpaceGenerate) { for (size_t i = 0; i < map.info.width; ++i) { - const int cost = cm.getCost(i, j, k); + const int cost = cm.getMapOverlay()->getCost(i, j, k); // All grid must be 0 ASSERT_EQ(cost, 0); // std::cout << std::setfill(' ') << std::setw(3) << cost << " "; @@ -236,7 +237,7 @@ TEST(Costmap3dLayerFootprintTest, testCSpaceGenerate) { for (size_t i = 0; i < map.info.width; ++i) { - const int cost = cm.getCost(i, j, k); + const int cost = cm.getMapOverlay()->getCost(i, j, k); // All grid must be 100 ASSERT_EQ(cost, 100); // std::cout << std::setfill(' ') << std::setw(3) << cost << " "; @@ -267,7 +268,7 @@ TEST(Costmap3dLayerFootprintTest, testCSpaceGenerate) { for (size_t i = 0; i < map.info.width; ++i) { - const int cost = cm.getCost(i, j, k); + const int cost = cm.getMapOverlay()->getCost(i, j, k); // Offset according to the template shape int cost_offset = 0; @@ -332,7 +333,7 @@ TEST(Costmap3dLayerFootprintTest, testCSpaceExpandSpread) { for (size_t i = 0; i < map.info.width; ++i) { - const int cost = cm.getCost(i, j, k); + const int cost = cm.getMapOverlay()->getCost(i, j, k); const float dist1 = hypotf(static_cast(i) - i_center, static_cast(j) - j_center); const float dist2 = hypotf(static_cast(i) - i_center2, static_cast(j) - j_center2); const float dist = std::min(dist1, dist2); @@ -445,9 +446,9 @@ TEST(Costmap3dLayerFootprintTest, testCSpaceOverwrite) for (size_t i = cm_over->getRangeMax(); i < map.info.width - cm_over->getRangeMax(); ++i) { const size_t addr = ((k * map.info.height + j) * map.info.width) + i; - assert(addr < updated.data.size()); + ROS_ASSERT(addr < updated.data.size()); const int cost = updated.data[addr]; - const int cost_ref = cm_ref.getCost(i, j, k); + const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k); ASSERT_EQ(cost, cost_ref); @@ -456,7 +457,7 @@ TEST(Costmap3dLayerFootprintTest, testCSpaceOverwrite) /*std::cout << " | "; for (int i = cm_over->getRangeMax(); i < map.info.width - cm_over->getRangeMax(); ++i) { - const int cost_ref = cm_ref.getCost(i, j, k); + const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k); std::cout << std::setfill(' ') << std::setw(3) << cost_ref << " "; } std::cout << std::endl; @@ -476,10 +477,10 @@ TEST(Costmap3dLayerFootprintTest, testCSpaceOverwrite) for (int i = cm_over->getRangeMax(); i < static_cast(map.info.width) - cm_over->getRangeMax(); ++i) { const size_t addr = ((k * map.info.height + j) * map.info.width) + i; - assert(addr < updated_max.data.size()); + ROS_ASSERT(addr < updated_max.data.size()); const int cost = updated_max.data[addr]; - const int cost_ref = cm_ref.getCost(i, j, k); - const int cost_base = cm_base.getCost(i, j, k); + const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k); + const int cost_base = cm_base.getMapOverlay()->getCost(i, j, k); const int cost_max = std::max(cost_ref, cost_base); ASSERT_EQ(cost, cost_max); @@ -490,13 +491,13 @@ TEST(Costmap3dLayerFootprintTest, testCSpaceOverwrite) std::cout << " | "; for (int i = cm_over->getRangeMax(); i < map.info.width - cm_over->getRangeMax(); ++i) { - const int cost_ref = cm_ref.getCost(i, j, k); + const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k); std::cout << std::setfill(' ') << std::setw(3) << cost_ref << " "; } std::cout << " | "; for (int i = cm_over->getRangeMax(); i < map.info.width - cm_over->getRangeMax(); ++i) { - const int cost_base = cm_base.getCost(i, j, k); + const int cost_base = cm_base.getMapOverlay()->getCost(i, j, k); std::cout << std::setfill(' ') << std::setw(3) << cost_base << " "; } std::cout << std::endl; @@ -561,7 +562,7 @@ TEST(Costmap3dLayerFootprintTest, testCSpaceOverlayMove) { for (size_t i = 0; i < map.info.width; ++i) { - const int cost = cm_over->getCost(i, j, k); + const int cost = cm_over->getMapOverlay()->getCost(i, j, k); if ((i == i_center && j == j_center) || (i == i_center2 && j == j_center2) ||