diff --git a/include/costmap_2d/costmap_layer.h b/include/costmap_2d/costmap_layer.h index 8514740..a2e5df0 100644 --- a/include/costmap_2d/costmap_layer.h +++ b/include/costmap_2d/costmap_layer.h @@ -39,6 +39,7 @@ #define COSTMAP_2D_COSTMAP_LAYER_H_ #include #include +#include namespace costmap_2d { @@ -70,7 +71,20 @@ public: */ void addExtraBounds(double mx0, double my0, double mx1, double my1); + template + void dataCallBack(const T& value) { + handle(value); + } + protected: + // Hàm template public, dùng để gửi dữ liệu + template + void handle(const T& data) { + handleImpl(static_cast(&data), typeid(T)); + } + + // Hàm ảo duy nhất mà plugin sẽ override + virtual void handleImpl(const void* data, const std::type_info& type) = 0; /* * Updates the master_grid within the specified * bounding box using this layer's values. diff --git a/include/costmap_2d/static_layer.h b/include/costmap_2d/static_layer.h index b36163d..7a86804 100644 --- a/include/costmap_2d/static_layer.h +++ b/include/costmap_2d/static_layer.h @@ -30,6 +30,10 @@ public: virtual void incomingMap(const nav_msgs::OccupancyGrid& new_map); virtual void incomingUpdate(const map_msgs::OccupancyGridUpdate& update); protected: + void handleImpl(const void* data, const std::type_info& type) override; + void process(const nav_msgs::OccupancyGrid& new_map); + void process(const map_msgs::OccupancyGridUpdate& update); + virtual unsigned char interpretValue(unsigned char value); unsigned char* threshold_; std::string base_frame_id_; diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp index 5f1bb8d..e30e124 100644 --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -362,7 +362,7 @@ void InflationLayer::setInflationParameters(double inflation_radius, double cost } // Export factory function -static PluginPtr create_inflation_plugin() { +static PluginCostmapLayerPtr create_inflation_plugin() { return std::make_shared(); } diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index 13e4cfa..cac1071 100644 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -1,5 +1,6 @@ #include #include +#include // #include #include #include @@ -17,8 +18,10 @@ namespace costmap_2d void ObstacleLayer::onInitialize() { + getParams(); // ros::NodeHandle nh("~/" + name_), g_nh; rolling_window_ = layered_costmap_->isRolling(); + bool track_unknown_space; // nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown()); @@ -115,7 +118,7 @@ void ObstacleLayer::onInitialize() > sub(new message_filters::Subscriber(g_nh, topic, 50)); boost::shared_ptr > filter( - new tf2_ros::MessageFilter(*sub, *tf_, global_frame_, 50, g_nh)); + new tf2_ros::MessageFilter(*sub, *tf_, global_frame_,50 , g_nh)); if (inf_is_valid) { @@ -190,13 +193,29 @@ void ObstacleLayer::onInitialize() ObstacleLayer::~ObstacleLayer() {} -// void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level) -// { -// enabled_ = config.enabled; -// footprint_clearing_enabled_ = config.footprint_clearing_enabled; -// max_obstacle_height_ = config.max_obstacle_height; -// combination_method_ = config.combination_method; -// } +bool StaticLayer::getParams() +{ + try { + YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml"); + YAML::Node layer = config["obstacle_layer"]; + + bool enabled = loadParam(layer, "enabled", true); + bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true); + double max_obstacle_height = loadParam(layer, "max_obstacle_height", 2); + int combination_method = loadParam(layer, "combination_method", 1); + + enabled_ = enabled; + footprint_clearing_enabled_ = footprint_clearing_enabled; + max_obstacle_height_ = max_obstacle_height; + combination_method_ = combination_method; + } + catch (const YAML::BadFile& e) { + std::cerr << "Cannot open YAML file: " << e.what() << std::endl; + return false; + } + + return true; +} // void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, // const boost::shared_ptr& buffer) diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index fdfeed3..3e1a87e 100644 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -160,6 +160,122 @@ unsigned char StaticLayer::interpretValue(unsigned char value) return scale * LETHAL_OBSTACLE; } +void StaticLayer::handleImpl(const void* data, const std::type_info& type) +{ + if (type == typeid(nav_msgs::OccupancyGrid)) { + process(*static_cast(data)); + } else if (type == typeid(map_msgs::OccupancyGridUpdate)) { + process(*static_cast(data)); + } else { + std::cout << "[Plugin] Unknown type: " << type.name() << std::endl; + } +} + +void StaticLayer::process(const nav_msgs::OccupancyGrid& new_map) +{ + if(!map_shutdown_) + { + std::cout << "Received new map!" << std::endl; + + unsigned int size_x = new_map.info.width, size_y = new_map.info.height; + + printf("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution); + + // resize costmap if size, resolution or origin do not match + Costmap2D* master = layered_costmap_->getCostmap(); + if (!layered_costmap_->isRolling() && + (master->getSizeInCellsX() != size_x || + master->getSizeInCellsY() != size_y || + master->getResolution() != new_map.info.resolution || + master->getOriginX() != new_map.info.origin.position.x || + master->getOriginY() != new_map.info.origin.position.y)) + { + // Update the size of the layered costmap (and all layers, including this one) + printf("Resizing costmap to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution); + layered_costmap_->resizeMap(size_x, size_y, new_map.info.resolution, new_map.info.origin.position.x, + new_map.info.origin.position.y, + true /* set size_locked to true, prevents reconfigureCb from overriding map size*/); + } + else if (size_x_ != size_x || size_y_ != size_y || + resolution_ != new_map.info.resolution || + origin_x_ != new_map.info.origin.position.x || + origin_y_ != new_map.info.origin.position.y) + { + // only update the size of the costmap stored locally in this layer + printf("Resizing static layer to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution); + resizeMap(size_x, size_y, new_map.info.resolution, + new_map.info.origin.position.x, new_map.info.origin.position.y); + } + + unsigned int index = 0; + + // initialize the costmap with static data + for (unsigned int i = 0; i < size_y; ++i) + { + for (unsigned int j = 0; j < size_x; ++j) + { + unsigned char value = new_map.data[index]; + costmap_[index] = interpretValue(value); + ++index; + } + } + map_frame_ = new_map.header.frame_id; + + // // Print current costmap + // std::cout << "[StaticLayer] Costmap (" << size_y_ << " x " << size_x_ << "):" << std::endl; + // for (unsigned int i = 0; i < size_y_; ++i) + // { + // for (unsigned int j = 0; j < size_x_; ++j) + // { + // unsigned int idx = i * size_x_ + j; + // std::cout << static_cast(costmap_[idx]); + // if (j + 1 < size_x_) std::cout << ' '; + // } + // std::cout << std::endl; + // } + + // we have a new map, update full size of map + x_ = y_ = 0; + width_ = size_x_; + height_ = size_y_; + map_received_ = true; + has_updated_data_ = true; + } + else + { + printf("Stop receive new map!"); + } + // shutdown the map subscrber if firt_map_only_ flag is on + if (first_map_only_) + { + printf("Shutting down the map subscriber. first_map_only flag is on"); + map_shutdown_ = true; + // map_sub_.shutdown(); + } +} +void StaticLayer::process(const map_msgs::OccupancyGridUpdate& update) +{ + if(!map_update_shutdown_) + { + std::cout << "Update new map!" << std::endl; + unsigned int di = 0; + for (unsigned int y = 0; y < update.height ; y++) + { + unsigned int index_base = (update.y + y) * size_x_; + for (unsigned int x = 0; x < update.width ; x++) + { + unsigned int index = index_base + x + update.x; + costmap_[index] = interpretValue(update.data[di++]); + } + } + x_ = update.x; + y_ = update.y; + width_ = update.width; + height_ = update.height; + has_updated_data_ = true; + } +} + void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map) { if(!map_shutdown_) diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index adde8ea..785ace7 100644 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -41,12 +41,12 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) std::string tf_error; - // if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf2::Time::now(), &tf_error)) - // { - // printf("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(), - // global_frame_.c_str(), tf_tolerance_, tf_error.c_str()); - // return false; - // } + if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf2::Time::now(), &tf_error)) + { + printf("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(), + global_frame_.c_str(), tf_tolerance_, tf_error.c_str()); + return false; + } list::iterator obs_it; for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) @@ -61,17 +61,17 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) origin.point = obs.origin_; // we need to transform the origin of the observation to the new global frame - // tf2::doTransform(origin, origin, - // tf2_buffer_.lookupTransform(new_global_frame, - // tf2::getFrameId(origin), - // tf2::getTimestamp(origin))); - // obs.origin_ = origin.point; + tf2::doTransform(origin, origin, + tf2_buffer_.lookupTransform(new_global_frame, + tf2::getFrameId(origin), + tf2::getTimestamp(origin))); + obs.origin_ = origin.point; - // // we also need to transform the cloud of the observation to the new global frame - // tf2::doTransform(*(obs.cloud_), *(obs.cloud_), - // tf2_buffer_.lookupTransform(new_global_frame, - // tf2::getFrameId(*(obs.cloud_)), - // tf2::getTimestamp(*(obs.cloud_)))); + // we also need to transform the cloud of the observation to the new global frame + tf2::doTransform(*(obs.cloud_), *(obs.cloud_), + tf2_buffer_.lookupTransform(new_global_frame, + tf2::getFrameId(*(obs.cloud_)), + tf2::getTimestamp(*(obs.cloud_)))); } catch (TransformException& ex) { @@ -105,10 +105,10 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud) local_origin.point.x = 0; local_origin.point.y = 0; local_origin.point.z = 0; - // tf2::doTransform(local_origin, global_origin, - // tf2_buffer_.lookupTransform(global_frame_, - // tf2::getFrameId(local_origin), - // tf2::getTimestamp(local_origin))); + tf2::doTransform(local_origin, global_origin, + tf2_buffer_.lookupTransform(global_frame_, + tf2::getFrameId(local_origin), + tf2::getTimestamp(local_origin))); tf2::convert(global_origin.point, observation_list_.front().origin_); // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations @@ -117,12 +117,12 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud) sensor_msgs::PointCloud2 global_frame_cloud; - // // transform the point cloud - // tf2::doTransform(cloud, global_frame_cloud, - // tf2_buffer_.lookupTransform(global_frame_, - // tf2::getFrameId(cloud), - // tf2::getTimestamp(cloud))); - // global_frame_cloud.header.stamp = cloud.header.stamp; + // transform the point cloud + tf2::doTransform(cloud, global_frame_cloud, + tf2_buffer_.lookupTransform(global_frame_, + tf2::getFrameId(cloud), + tf2::getTimestamp(cloud))); + global_frame_cloud.header.stamp = cloud.header.stamp; // now we need to remove observations from the cloud that are below or above our height thresholds sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_); diff --git a/test/static_layer_test.cpp b/test/static_layer_test.cpp index 0e43536..2e5af92 100644 --- a/test/static_layer_test.cpp +++ b/test/static_layer_test.cpp @@ -5,11 +5,11 @@ #include #include #include "nav_msgs/OccupancyGrid.h" +#include using namespace costmap_2d; int main(int argc, char* argv[]) { - // Fix 1: Use correct path to the library auto creator = boost::dll::import_alias( "./costmap_2d/liblayers.so", "create_plugin", boost::dll::load_mode::append_decorations ); @@ -17,24 +17,19 @@ int main(int argc, char* argv[]) { costmap_2d::PluginCostmapLayerPtr plugin = creator(); std::cout << "Plugin created successfully" << std::endl; - // Fix 2: Initialize the plugin before calling activate() - // activate() calls onInitialize() which requires layered_costmap_ to be set std::string global_frame = "map"; - bool rolling_window = false; + bool rolling_window = true; bool track_unknown = true; LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown); - // Fix 3: tf2::BufferCore constructor requires ros::Duration parameter (not tf2::Duration) tf2::BufferCore tf_buffer; - // tf2::Duration cache_time(10.0); // 10 seconds cache time + // tf2::Duration cache_time(10.0); // tf2::BufferCore tf_buffer(cache_time); - // Initialize the layer with required parameters plugin->initialize(&layered_costmap, "static_layer", &tf_buffer); std::cout << "Plugin initialized" << std::endl; - // Now it's safe to call activate() // plugin->activate(); nav_msgs::OccupancyGrid grid; @@ -43,11 +38,28 @@ int main(int argc, char* argv[]) { grid.info.height = 2; grid.data.resize(grid.info.width * grid.info.height, -1); - grid.data[0] = 0; // Free cell - grid.data[1] = 100; // Occupied cell - grid.data[2] = 10; // Occupied cell - grid.data[3] = 0; // Free cell - plugin->incomingMap(grid); + grid.data[0] = 0; + grid.data[1] = 100; + grid.data[2] = 10; + grid.data[3] = 0; + plugin->dataCallBack(grid); + + map_msgs::OccupancyGridUpdate update; + + update.x = 1; + update.y = 1; + update.width = 2; + update.height = 2; + + update.data.resize(update.width * update.height, -1); + update.data[0] = 0; + update.data[1] = 100; + update.data[2] = 10; + update.data[3] = 0; + plugin->dataCallBack(update); + + + std::cout << "Plugin activated successfully" << std::endl; }