From c94de60a7bc4645d3b351452ad473941c9a478a8 Mon Sep 17 00:00:00 2001 From: duongtd Date: Wed, 12 Nov 2025 17:38:38 +0700 Subject: [PATCH] update file obstacle_layer and file test plugin --- CMakeLists.txt | 30 +- include/costmap_2d/costmap_layer.h | 12 +- include/costmap_2d/critical_layer copy.h | 19 - include/costmap_2d/layer.h | 1 + include/costmap_2d/obstacle_layer.h | 18 +- include/costmap_2d/static_layer.h | 13 +- plugins/critical_layer.cpp | 4 +- plugins/inflation_layer.cpp | 4 +- plugins/obstacle_layer.cpp | 504 ++++++++++------------- plugins/static_layer.cpp | 178 +------- src/observation_buffer.cpp | 31 +- test/static_layer_test.cpp | 96 ++++- 12 files changed, 393 insertions(+), 517 deletions(-) delete mode 100644 include/costmap_2d/critical_layer copy.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 636e5d6..1d06b41 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -147,6 +147,9 @@ endif() if (NOT TARGET map_msgs) add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../map_msgs ${CMAKE_BINARY_DIR}/map_msgs_build) endif() +if (NOT TARGET laser_geometry) + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../laser_geometry ${CMAKE_BINARY_DIR}/laser_geometry_build) +endif() include_directories( include @@ -179,20 +182,40 @@ target_link_libraries(costmap_2d_new geometry_msgs nav_msgs map_msgs + laser_geometry ${TF2_LIBRARY} ) target_include_directories(costmap_2d_new PRIVATE ${Boost_INCLUDE_DIRS}) # --- Plugin libraries --- -add_library(layers SHARED +add_library(static_layer SHARED plugins/static_layer.cpp ) -target_link_libraries(layers +target_link_libraries(static_layer costmap_2d_new ${Boost_LIBRARIES} yaml-cpp ) +add_library(obstacle_layer SHARED + plugins/obstacle_layer.cpp +) +target_link_libraries(obstacle_layer + costmap_2d_new + ${Boost_LIBRARIES} + yaml-cpp +) + +add_library(inflation_layer SHARED + plugins/inflation_layer.cpp +) +target_link_libraries(inflation_layer + costmap_2d_new + ${Boost_LIBRARIES} + yaml-cpp +) + + # --- Test executables --- add_executable(test_array_parser test/array_parser_test.cpp) add_executable(test_costmap test/coordinates_test.cpp) @@ -215,7 +238,8 @@ target_link_libraries(test_costmap PRIVATE target_link_libraries(test_plugin PRIVATE ${TF2_LIBRARY} costmap_2d_new - layers + static_layer + obstacle_layer ${Boost_LIBRARIES} Boost::filesystem Boost::system diff --git a/include/costmap_2d/costmap_layer.h b/include/costmap_2d/costmap_layer.h index a2e5df0..f327fe7 100644 --- a/include/costmap_2d/costmap_layer.h +++ b/include/costmap_2d/costmap_layer.h @@ -72,19 +72,21 @@ public: void addExtraBounds(double mx0, double my0, double mx1, double my1); template - void dataCallBack(const T& value) { - handle(value); + void dataCallBack(const T& value, const std::string& topic) { + handle(value, topic); } protected: // Hàm template public, dùng để gửi dữ liệu template - void handle(const T& data) { - handleImpl(static_cast(&data), typeid(T)); + void handle(const T& data, const std::string& topic) { + handleImpl(static_cast(&data), typeid(T), topic); } // Hàm ảo duy nhất mà plugin sẽ override - virtual void handleImpl(const void* data, const std::type_info& type) = 0; + virtual void handleImpl(const void* data, + const std::type_info& type, + const std::string& topic) = 0; /* * Updates the master_grid within the specified * bounding box using this layer's values. diff --git a/include/costmap_2d/critical_layer copy.h b/include/costmap_2d/critical_layer copy.h deleted file mode 100644 index eb4e3ee..0000000 --- a/include/costmap_2d/critical_layer copy.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef COSTMAP_2D_CRITICAL_LAYER_H_ -#define COSTMAP_2D_CRITICAL_LAYER_H_ - -#include - -namespace costmap_2d -{ -class CriticalLayer : public StaticLayer -{ -public: - CriticalLayer(); - virtual ~CriticalLayer(); -private: - unsigned char interpretValue(unsigned char value); - void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); -}; -} - -#endif // COSTMAP_2D_CRITICAL_LAYER_H_ \ No newline at end of file diff --git a/include/costmap_2d/layer.h b/include/costmap_2d/layer.h index 6890bbd..5ad9c00 100644 --- a/include/costmap_2d/layer.h +++ b/include/costmap_2d/layer.h @@ -146,6 +146,7 @@ private: std::vector footprint_spec_; }; +using PluginLayerPtr = std::shared_ptr; } // namespace costmap_2d diff --git a/include/costmap_2d/obstacle_layer.h b/include/costmap_2d/obstacle_layer.h index 4eff519..01cd37a 100644 --- a/include/costmap_2d/obstacle_layer.h +++ b/include/costmap_2d/obstacle_layer.h @@ -51,7 +51,7 @@ #include #include -// #include +#include // #include // #include @@ -114,6 +114,19 @@ public: protected: + void handleImpl(const void* data, + const std::type_info&, + const std::string& topic) override; + void laserScanCallback(const sensor_msgs::LaserScan& message, + const boost::shared_ptr& buffer); + void laserScanValidInfCallback(const sensor_msgs::LaserScan& message, + const boost::shared_ptr& buffer); + void pointCloudCallback(const sensor_msgs::PointCloud& message, + const boost::shared_ptr& buffer); + void pointCloud2Callback(const sensor_msgs::PointCloud2& message, + const boost::shared_ptr& buffer); + // void process(const map_msgs::OccupancyGridUpdate& update); + /** * @brief Get the observations used to mark space * @param marking_observations A reference to a vector that will be populated with the observations @@ -150,7 +163,7 @@ protected: std::string global_frame_; ///< @brief The global frame for the costmap double max_obstacle_height_; ///< @brief Max Obstacle Height - // laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds + laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds // std::vector > observation_subscribers_; ///< @brief Used for the observation message filters // std::vector > observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor @@ -162,6 +175,7 @@ protected: std::vector static_clearing_observations_, static_marking_observations_; bool rolling_window_; + bool stop_receiving_data_ = false; int combination_method_; diff --git a/include/costmap_2d/static_layer.h b/include/costmap_2d/static_layer.h index 7a86804..87c55c9 100644 --- a/include/costmap_2d/static_layer.h +++ b/include/costmap_2d/static_layer.h @@ -5,7 +5,6 @@ #include #include #include -// #include #include #include @@ -27,12 +26,12 @@ public: virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); virtual void matchSize(); - 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); + void handleImpl(const void* data, + const std::type_info& type, + const std::string& topic) override; + void incomingMap(const nav_msgs::OccupancyGrid& new_map); + void incomingUpdate(const map_msgs::OccupancyGridUpdate& update); virtual unsigned char interpretValue(unsigned char value); unsigned char* threshold_; @@ -65,6 +64,8 @@ private: bool getParams(); }; +using PluginStaticLayerPtr = std::shared_ptr; + } // namespace costmap_2d #endif // COSTMAP_2D_STATIC_LAYER_H_ diff --git a/plugins/critical_layer.cpp b/plugins/critical_layer.cpp index 2229445..ab6e74c 100644 --- a/plugins/critical_layer.cpp +++ b/plugins/critical_layer.cpp @@ -34,10 +34,10 @@ void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, i } // Export factory function -static PluginPtr create_critical_plugin() { +static PluginStaticLayerPtr create_critical_plugin() { return std::make_shared(); } // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) -BOOST_DLL_ALIAS(create_critical_plugin, create_plugin) +BOOST_DLL_ALIAS(create_critical_plugin, create_plugin_static_layer) } \ No newline at end of file diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp index e30e124..8fb12ef 100644 --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -362,11 +362,11 @@ void InflationLayer::setInflationParameters(double inflation_radius, double cost } // Export factory function -static PluginCostmapLayerPtr create_inflation_plugin() { +static PluginLayerPtr create_inflation_plugin() { return std::make_shared(); } // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) -BOOST_DLL_ALIAS(create_inflation_plugin, create_plugin) +BOOST_DLL_ALIAS(create_inflation_plugin, create_plugin_layer) } // namespace costmap_2d diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index cac1071..ee03361 100644 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -1,8 +1,8 @@ #include #include #include -// #include #include +#include #include #include @@ -18,196 +18,94 @@ 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()); - if (track_unknown_space) - default_value_ = NO_INFORMATION; - else - default_value_ = FREE_SPACE; - ObstacleLayer::matchSize(); current_ = true; - + stop_receiving_data_ = false; global_frame_ = layered_costmap_->getGlobalFrameID(); - double transform_tolerance; -// nh.param("transform_tolerance", transform_tolerance, 0.2); - - std::string topics_string; - // get the topics that we'll subscribe to from the parameter server -// nh.param("observation_sources", topics_string, std::string("")); -// ROS_INFO(" Subscribed to Topics: %s", topics_string.c_str()); - - // now we need to split the topics based on whitespace which we can use a stringstream for - std::stringstream ss(topics_string); -/* - std::string source; - while (ss >> source) - { - ros::NodeHandle source_node(nh, source); - - // get the parameters for the specific topic - double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height; - std::string topic, sensor_frame, data_type; - bool inf_is_valid, clearing, marking; - - source_node.param("topic", topic, source); - source_node.param("sensor_frame", sensor_frame, std::string("")); - source_node.param("observation_persistence", observation_keep_time, 0.0); - source_node.param("expected_update_rate", expected_update_rate, 0.0); - source_node.param("data_type", data_type, std::string("PointCloud")); - source_node.param("min_obstacle_height", min_obstacle_height, 0.0); - source_node.param("max_obstacle_height", max_obstacle_height, 2.0); - source_node.param("inf_is_valid", inf_is_valid, false); - source_node.param("clearing", clearing, false); - source_node.param("marking", marking, true); - - if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")) - { - ROS_FATAL("Only topics that use point clouds or laser scans are currently supported"); - throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported"); - } - - std::string raytrace_range_param_name, obstacle_range_param_name; - - // get the obstacle range for the sensor - double obstacle_range = 2.5; - if (source_node.searchParam("obstacle_range", obstacle_range_param_name)) - { - source_node.getParam(obstacle_range_param_name, obstacle_range); - } - - // get the raytrace range for the sensor - double raytrace_range = 3.0; - if (source_node.searchParam("raytrace_range", raytrace_range_param_name)) - { - source_node.getParam(raytrace_range_param_name, raytrace_range); - } - - printf("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(), - sensor_frame.c_str()); - - // create an observation buffer - observation_buffers_.push_back( - boost::shared_ptr < ObservationBuffer - > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, - max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, - sensor_frame, transform_tolerance))); - - // check if we'll add this buffer to our marking observation buffers - if (marking) - marking_buffers_.push_back(observation_buffers_.back()); - - // check if we'll also add this buffer to our clearing observation buffers - if (clearing) - clearing_buffers_.push_back(observation_buffers_.back()); - - printf( - "Created an observation buffer for source %s, topic %s, global frame: %s, " - "expected update rate: %.2f, observation persistence: %.2f", - source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time); - - // create a callback for the topic - if (data_type == "LaserScan") - { - boost::shared_ptr < message_filters::Subscriber - > sub(new message_filters::Subscriber(g_nh, topic, 50)); - - boost::shared_ptr > filter( - new tf2_ros::MessageFilter(*sub, *tf_, global_frame_,50 , g_nh)); - - if (inf_is_valid) - { - filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); }); - } - else - { - filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); }); - } - - observation_subscribers_.push_back(sub); - observation_notifiers_.push_back(filter); - - observation_notifiers_.back()->setTolerance(ros::Duration(0.05)); - } - else if (data_type == "PointCloud") - { - boost::shared_ptr < message_filters::Subscriber - > sub(new message_filters::Subscriber(g_nh, topic, 50)); - - if (inf_is_valid) - { - ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations."); - } - - boost::shared_ptr < tf2_ros::MessageFilter - > filter(new tf2_ros::MessageFilter(*sub, *tf_, global_frame_, 50, g_nh)); - filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); }); - - observation_subscribers_.push_back(sub); - observation_notifiers_.push_back(filter); - } - else - { - boost::shared_ptr < message_filters::Subscriber - > sub(new message_filters::Subscriber(g_nh, topic, 50)); - - if (inf_is_valid) - { - ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations."); - } - - boost::shared_ptr < tf2_ros::MessageFilter - > filter(new tf2_ros::MessageFilter(*sub, *tf_, global_frame_, 50, g_nh)); - filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); }); - - observation_subscribers_.push_back(sub); - observation_notifiers_.push_back(filter); - } - - if (sensor_frame != "") - { - std::vector < std::string > target_frames; - target_frames.push_back(global_frame_); - target_frames.push_back(sensor_frame); - observation_notifiers_.back()->setTargetFrames(target_frames); - } - } -*/ -// dsrv_ = NULL; -// setupDynamicReconfigure(nh); + getParams(); } -// void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh) -// { -// dsrv_ = new dynamic_reconfigure::Server(nh); -// dynamic_reconfigure::Server::CallbackType cb = -// [this](auto& config, auto level){ reconfigureCB(config, level); }; -// dsrv_->setCallback(cb); -// } ObstacleLayer::~ObstacleLayer() {} -bool StaticLayer::getParams() +bool ObstacleLayer::getParams() { try { - YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml"); - YAML::Node layer = config["obstacle_layer"]; + 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); + bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown()); + if (track_unknown_space) + default_value_ = NO_INFORMATION; + else + default_value_ = FREE_SPACE; + + double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2); + // get the topics that we'll subscribe to from the parameter server + std::string topics_string = loadParam(layer,"observation_sources", std::string("")); + printf(" Subscribed to Topics: %s\n", topics_string.c_str()); + + // get the parameters for the specific topic + double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2; + std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud"; + bool inf_is_valid = false, clearing=false, marking=true; + topic = loadParam(layer,"topic", topic); + sensor_frame = loadParam(layer,"sensor_frame", std::string("")); + observation_keep_time = loadParam(layer,"observation_persistence", 0.0); + expected_update_rate = loadParam(layer,"expected_update_rate", 0.0); + data_type = loadParam(layer,"data_type", std::string("PointCloud")); + min_obstacle_height = loadParam(layer,"min_obstacle_height", 0.0); + max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0); + inf_is_valid = loadParam(layer,"inf_is_valid", false); + clearing = loadParam(layer,"clearing", false); + marking = loadParam(layer,"marking", true); + if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")) + { + printf("Only topics that use point clouds or laser scans are currently supported\n"); + throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported"); + } + + std::string raytrace_range_param_name, obstacle_range_param_name; + + double obstacle_range = 2.5; + obstacle_range = loadParam(layer,"obstacle_range", obstacle_range); + double raytrace_range = 3.0; + raytrace_range = loadParam(layer,"raytrace_range", raytrace_range); + + + bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true); + 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; + + printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(), + sensor_frame.c_str()); + + // create an observation buffer + observation_buffers_.push_back( + boost::shared_ptr < ObservationBuffer + > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, + max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, + sensor_frame, transform_tolerance))); + if (marking) + marking_buffers_.push_back(observation_buffers_.back()); + + // check if we'll also add this buffer to our clearing observation buffers + if (clearing) + clearing_buffers_.push_back(observation_buffers_.back()); + + printf( + "Created an observation buffer for topic %s, global frame: %s, " + "expected update rate: %.2f, observation persistence: %.2f", + topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time); - 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; @@ -217,103 +115,133 @@ bool StaticLayer::getParams() return true; } -// void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, -// const boost::shared_ptr& buffer) -// { -// // project the laser into a point cloud -// sensor_msgs::PointCloud2 cloud; -// cloud.header = message->header; +void ObstacleLayer::handleImpl(const void* data, + const std::type_info& type, + const std::string& topic) +{ + if(!stop_receiving_data_) + { + if(observation_buffers_.empty()) return; + boost::shared_ptr buffer = observation_buffers_.back(); + if (type == typeid(sensor_msgs::LaserScan) && topic == "laser") { + laserScanCallback(*static_cast(data), buffer); + } else if (type == typeid(sensor_msgs::LaserScan) && topic == "laser_valid") { + laserScanValidInfCallback(*static_cast(data), buffer); + } else if (type == typeid(sensor_msgs::PointCloud) && topic == "pcl_cb") { + pointCloudCallback(*static_cast(data), buffer); + } else if (type == typeid(sensor_msgs::PointCloud2) && topic == "pcl2_cb") { + pointCloud2Callback(*static_cast(data), buffer); + } else { + std::cout << "[Plugin] Unknown type: " << type.name() << std::endl; + } + } + else + { + std::cout << "Stop receiving data!" << std::endl; + return; + } +} -// // project the scan into a point cloud -// try -// { -// projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_); -// } -// catch (tf2::TransformException &ex) -// { -// ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(), -// ex.what()); -// projector_.projectLaser(*message, cloud); -// } -// catch (std::runtime_error &ex) -// { -// ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what()); -// return; //ignore this message -// } +void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message, + const boost::shared_ptr& buffer) +{ + // project the laser into a point cloud + sensor_msgs::PointCloud2 cloud; + cloud.header = message.header; + // printf("TEST PLUGIN OBSTACLE!!!\n"); -// // buffer the point cloud -// buffer->lock(); -// buffer->bufferCloud(cloud); -// buffer->unlock(); -// } + // project the scan into a point cloud + try + { + projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_); + } + catch (tf2::TransformException &ex) + { + printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(), + ex.what()); + projector_.projectLaser(message, cloud); + } + catch (std::runtime_error &ex) + { + printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what()); + return; //ignore this message + } -// void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message, -// const boost::shared_ptr& buffer) -// { -// // Filter positive infinities ("Inf"s) to max_range. -// float epsilon = 0.0001; // a tenth of a millimeter -// sensor_msgs::LaserScan message = *raw_message; -// for (size_t i = 0; i < message.ranges.size(); i++) -// { -// float range = message.ranges[ i ]; -// if (!std::isfinite(range) && range > 0) -// { -// message.ranges[ i ] = message.range_max - epsilon; -// } -// } + // buffer the point cloud + buffer->lock(); + buffer->bufferCloud(cloud); + buffer->unlock(); +} -// // project the laser into a point cloud -// sensor_msgs::PointCloud2 cloud; -// cloud.header = message.header; +void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message, + const boost::shared_ptr& buffer) +{ + // printf("TEST PLUGIN OBSTACLE 2!!!\n"); + // Filter positive infinities ("Inf"s) to max_range. + float epsilon = 0.0001; // a tenth of a millimeter + sensor_msgs::LaserScan message = raw_message; + for (size_t i = 0; i < message.ranges.size(); i++) + { + float range = message.ranges[ i ]; + if (!std::isfinite(range) && range > 0) + { + message.ranges[ i ] = message.range_max - epsilon; + } + } -// // project the scan into a point cloud -// try -// { -// projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_); -// } -// catch (tf2::TransformException &ex) -// { -// ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", -// global_frame_.c_str(), ex.what()); -// projector_.projectLaser(message, cloud); -// } -// catch (std::runtime_error &ex) -// { -// ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what()); -// return; //ignore this message -// } + // project the laser into a point cloud + sensor_msgs::PointCloud2 cloud; + cloud.header = message.header; -// // buffer the point cloud -// buffer->lock(); -// buffer->bufferCloud(cloud); -// buffer->unlock(); -// } + // project the scan into a point cloud + try + { + projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_); + } + catch (tf2::TransformException &ex) + { + printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", + global_frame_.c_str(), ex.what()); + projector_.projectLaser(message, cloud); + } + catch (std::runtime_error &ex) + { + printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what()); + return; //ignore this message + } -// void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message, -// const boost::shared_ptr& buffer) -// { -// sensor_msgs::PointCloud2 cloud2; + // buffer the point cloud + buffer->lock(); + buffer->bufferCloud(cloud); + buffer->unlock(); +} -// if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2)) -// { -// ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message"); -// return; -// } +void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message, + const boost::shared_ptr& buffer) +{ + // printf("TEST PLUGIN OBSTACLE 3!!!\n"); + sensor_msgs::PointCloud2 cloud2; -// // buffer the point cloud -// buffer->lock(); -// buffer->bufferCloud(cloud2); -// buffer->unlock(); -// } + if (!sensor_msgs::convertPointCloudToPointCloud2(message, cloud2)) + { + printf("Failed to convert a PointCloud to a PointCloud2, dropping message\n"); + return; + } -// void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message, -// const boost::shared_ptr& buffer) -// { -// // buffer the point cloud -// buffer->lock(); -// buffer->bufferCloud(*message); -// buffer->unlock(); -// } + // buffer the point cloud + buffer->lock(); + buffer->bufferCloud(cloud2); + buffer->unlock(); +} + +void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2& message, + const boost::shared_ptr& buffer) +{ + // buffer the point cloud + buffer->lock(); + buffer->bufferCloud(message); + buffer->unlock(); +} void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) @@ -360,7 +288,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya // if the obstacle is too high or too far away from the robot we won't add it if (pz > max_obstacle_height_) { - printf("The point is too high"); + printf("The point is too high\n"); continue; } @@ -371,7 +299,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya // if the point is far enough away... we won't consider it if (sq_dist >= sq_obstacle_range) { - printf("The point is too far away"); + printf("The point is too far away\n"); continue; } @@ -379,7 +307,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya unsigned int mx, my; if (!worldToMap(px, py, mx, my)) { - printf("Computing map coords failed"); + printf("Computing map coords failed\n"); continue; } @@ -485,7 +413,7 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d if (!worldToMap(ox, oy, x0, y0)) { printf( - "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", + "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n", ox, oy); return; } @@ -556,29 +484,19 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d } } -// void ObstacleLayer::activate() -// { -// // if we're stopped we need to re-subscribe to topics -// for (unsigned int i = 0; i < observation_subscribers_.size(); ++i) -// { -// if (observation_subscribers_[i] != NULL) -// observation_subscribers_[i]->subscribe(); -// } - -// for (unsigned int i = 0; i < observation_buffers_.size(); ++i) -// { -// if (observation_buffers_[i]) -// observation_buffers_[i]->resetLastUpdated(); -// } -// } -// void ObstacleLayer::deactivate() -// { -// for (unsigned int i = 0; i < observation_subscribers_.size(); ++i) -// { -// if (observation_subscribers_[i] != NULL) -// observation_subscribers_[i]->unsubscribe(); -// } -// } +void ObstacleLayer::activate() +{ + stop_receiving_data_ = false; + for (unsigned int i = 0; i < observation_buffers_.size(); ++i) + { + if (observation_buffers_[i]) + observation_buffers_[i]->resetLastUpdated(); + } +} +void ObstacleLayer::deactivate() +{ + stop_receiving_data_ = true; +} void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y, double* max_x, double* max_y) @@ -590,16 +508,16 @@ void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double touch(ex, ey, min_x, min_y, max_x, max_y); } -// void ObstacleLayer::reset() -// { +void ObstacleLayer::reset() +{ // deactivate(); // resetMaps(); // current_ = true; // activate(); -// } +} // Export factory function -static PluginPtr create_obstacle_plugin() { +static PluginCostmapLayerPtr create_obstacle_plugin() { return std::make_shared(); } diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index 3e1a87e..d825bda 100644 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -5,7 +5,6 @@ #include #include #include -// #include #include @@ -26,55 +25,10 @@ StaticLayer::~StaticLayer() void StaticLayer::onInitialize() { - // ros::NodeHandle nh("~/" + name_), g_nh; current_ = true; global_frame_ = layered_costmap_->getGlobalFrameID(); getParams(); - // nh.param("map_topic", map_topic_, std::string("map")); - // nh.param("first_map_only", first_map_only_, false); - // nh.param("subscribe_to_updates", subscribe_to_updates_, false); - - // nh.param("track_unknown_space", track_unknown_space_, true); - // nh.param("use_maximum", use_maximum_, false); - - // int temp_lethal_threshold, temp_unknown_cost_value; - // nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100)); - // nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1)); - // nh.param("trinary_costmap", trinary_costmap_, true); - // nh.param("base_frame_id", base_frame_id_, std::string("base_footprint")); - - // lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); - // unknown_cost_value_ = temp_unknown_cost_value; - - // Only resubscribe if topic has changed - // if (map_sub_.getTopic() != ros::names::resolve(map_topic_)) - // { - // // we'll subscribe to the latched topic that the map server uses - // printf("Requesting the map..."); - // map_sub_ = g_nh.subscribe(map_topic_, 1, &StaticLayer::incomingMap, this); - // map_received_ = false; - // has_updated_data_ = false; - - // ros::Rate r(10); - // while (!map_received_ && g_nh.ok()) - // { - // ros::spinOnce(); - // r.sleep(); - // } - - // printf("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution()); - - // if (subscribe_to_updates_) - // { - std::cout<<"Subscribing to updates"<(data)); - } else if (type == typeid(map_msgs::OccupancyGridUpdate)) { - process(*static_cast(data)); + if (type == typeid(nav_msgs::OccupancyGrid) && topic == "map") { + incomingMap(*static_cast(data)); + } else if (type == typeid(map_msgs::OccupancyGridUpdate) && topic == "map_update") { + incomingUpdate(*static_cast(data)); } else { std::cout << "[Plugin] Unknown type: " << type.name() << std::endl; } } -void StaticLayer::process(const nav_msgs::OccupancyGrid& new_map) +void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map) { if(!map_shutdown_) { @@ -221,25 +166,13 @@ void StaticLayer::process(const nav_msgs::OccupancyGrid& new_map) } 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 { @@ -253,7 +186,8 @@ void StaticLayer::process(const nav_msgs::OccupancyGrid& new_map) // map_sub_.shutdown(); } } -void StaticLayer::process(const map_msgs::OccupancyGridUpdate& update) + +void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update) { if(!map_update_shutdown_) { @@ -276,94 +210,10 @@ void StaticLayer::process(const map_msgs::OccupancyGridUpdate& update) } } -void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map) -{ - if(!map_shutdown_) - { - 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", 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", 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", 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; - - // 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; - } - // 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::incomingUpdate(const map_msgs::OccupancyGridUpdate& update) -{ - if(!map_update_shutdown_) - { - 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::activate() { + map_shutdown_ = false; + map_update_shutdown_ = false; onInitialize(); } diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index 785ace7..6432a3f 100644 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -1,5 +1,5 @@ #include - +#include // #include // #include #include @@ -32,18 +32,13 @@ ObservationBuffer::~ObservationBuffer() bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) { - geometry_msgs::Point A; - // ros::Time transform_time = ros::Time::now(); - // double transform_time = - // std::chrono::duration( - // std::chrono::system_clock::now().time_since_epoch()).count(); robot::Time transform_time = robot::Time::now(); 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(), + printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(), global_frame_.c_str(), tf_tolerance_, tf_error.c_str()); return false; } @@ -63,19 +58,19 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) // 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))); + origin.header.frame_id, + convertTime(origin.header.stamp))); 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_)))); + obs.cloud_->header.frame_id, + convertTime(obs.cloud_->header.stamp))); } catch (TransformException& ex) { - printf("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(), + printf("TF Error attempting to transform an observation from %s to %s: %s\n", global_frame_.c_str(), new_global_frame.c_str(), ex.what()); return false; } @@ -107,8 +102,8 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud) local_origin.point.z = 0; tf2::doTransform(local_origin, global_origin, tf2_buffer_.lookupTransform(global_frame_, - tf2::getFrameId(local_origin), - tf2::getTimestamp(local_origin))); + local_origin.header.frame_id, + convertTime(local_origin.header.stamp))); 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 @@ -120,8 +115,8 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud) // transform the point cloud tf2::doTransform(cloud, global_frame_cloud, tf2_buffer_.lookupTransform(global_frame_, - tf2::getFrameId(cloud), - tf2::getTimestamp(cloud))); + (cloud.header.frame_id), + convertTime(cloud.header.stamp))); 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 @@ -163,7 +158,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud) { // if an exception occurs, we need to remove the empty observation from the list observation_list_.pop_front(); - printf("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(), + printf("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what()); return; } @@ -224,7 +219,7 @@ bool ObservationBuffer::isCurrent() const if (!current) { printf( - "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.", + "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n", topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec()); } diff --git a/test/static_layer_test.cpp b/test/static_layer_test.cpp index 2e5af92..7fbac41 100644 --- a/test/static_layer_test.cpp +++ b/test/static_layer_test.cpp @@ -6,12 +6,13 @@ #include #include "nav_msgs/OccupancyGrid.h" #include +#include using namespace costmap_2d; int main(int argc, char* argv[]) { auto creator = boost::dll::import_alias( - "./costmap_2d/liblayers.so", "create_plugin", boost::dll::load_mode::append_decorations + "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations ); costmap_2d::PluginCostmapLayerPtr plugin = creator(); @@ -42,7 +43,7 @@ int main(int argc, char* argv[]) { grid.data[1] = 100; grid.data[2] = 10; grid.data[3] = 0; - plugin->dataCallBack(grid); + plugin->dataCallBack(grid, "map"); map_msgs::OccupancyGridUpdate update; @@ -56,7 +57,96 @@ int main(int argc, char* argv[]) { update.data[1] = 100; update.data[2] = 10; update.data[3] = 0; - plugin->dataCallBack(update); + plugin->dataCallBack(update, "map_update"); + + + creator = boost::dll::import_alias( + "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations + ); + + plugin = creator(); + std::cout << "Plugin created successfully" << std::endl; + + plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer); + + sensor_msgs::LaserScan scan; + + // --- Header --- + scan.header.seq = 1; + scan.header.stamp = robot::Time::now(); // nếu bạn chưa có Time class, gán tạm số + scan.header.frame_id = "laser_frame"; + + // --- Cấu hình góc --- + scan.angle_min = -M_PI; // -180 độ + scan.angle_max = M_PI; // +180 độ + scan.angle_increment = M_PI / 180; // 1 độ mỗi tia + + // --- Cấu hình thời gian --- + scan.time_increment = 0.0001f; // mỗi tia cách nhau 0.1 ms + scan.scan_time = 0.04f; // tổng thời gian quét 40 ms (~25Hz) + + // --- Giới hạn đo --- + scan.range_min = 0.1f; + scan.range_max = 10.0f; + + // --- Tạo dữ liệu --- + int num_readings = static_cast((scan.angle_max - scan.angle_min) / scan.angle_increment); + scan.ranges.resize(num_readings); + scan.intensities.resize(num_readings); + + for (int i = 0; i < num_readings; ++i) + { + float angle = scan.angle_min + i * scan.angle_increment; + + // Tạo dữ liệu giả lập: vật thể cách 2m, dao động nhẹ theo góc + scan.ranges[i] = 2.0f + 0.2f * std::sin(3.0f * angle); + + // Giả lập cường độ phản xạ + scan.intensities[i] = 100.0f + 20.0f * std::cos(angle); + } + + // --- In thử 10 giá trị đầu --- + // for (int i = 0; i < 10; ++i) + // { + // std::cout << "Angle " << (scan.angle_min + i * scan.angle_increment) + // << " rad | Range " << scan.ranges[i] + // << " m | Intensity " << scan.intensities[i] + // << std::endl; + // } + plugin->deactivate(); + plugin->dataCallBack(scan, "laser_valid"); + + sensor_msgs::PointCloud cloud; + + // 2. Thiết lập header + cloud.header.stamp.sec = 1234567890; // hoặc ros::Time::now().sec + cloud.header.stamp.nsec = 0; + cloud.header.frame_id = "laser_frame"; + + // 3. Thêm một vài điểm + geometry_msgs::Point32 pt1; + pt1.x = 1.0f; pt1.y = 0.0f; pt1.z = 0.0f; + + geometry_msgs::Point32 pt2; + pt2.x = 0.0f; pt2.y = 1.0f; pt2.z = 0.0f; + + geometry_msgs::Point32 pt3; + pt3.x = 1.0f; pt3.y = 1.0f; pt3.z = 0.0f; + + cloud.points.push_back(pt1); + cloud.points.push_back(pt2); + cloud.points.push_back(pt3); + + // 4. Thêm dữ liệu channels (tùy chọn) + cloud.channels.resize(1); + cloud.channels[0].name = "intensity"; + cloud.channels[0].values.push_back(0.5f); + cloud.channels[0].values.push_back(1.0f); + cloud.channels[0].values.push_back(0.8f); + + plugin->dataCallBack(cloud, "pcl_cb"); + +