From ae469e3271bdcdb4f20629dd7de57553c1e7e074 Mon Sep 17 00:00:00 2001 From: duongtd Date: Wed, 31 Dec 2025 10:11:12 +0700 Subject: [PATCH] add function getTypeLayer --- include/robot_costmap_2d/critical_layer.h | 4 ++++ include/robot_costmap_2d/directional_layer.h | 5 ++++- include/robot_costmap_2d/inflation_layer.h | 5 +++++ include/robot_costmap_2d/layer.h | 19 ++++++++++++++++++- include/robot_costmap_2d/obstacle_layer.h | 5 +++++ include/robot_costmap_2d/preferred_layer.h | 4 ++++ include/robot_costmap_2d/static_layer.h | 6 ++++++ include/robot_costmap_2d/unpreferred_layer.h | 5 ++++- include/robot_costmap_2d/voxel_layer.h | 5 ++++- plugins/static_layer.cpp | 2 +- src/observation_buffer.cpp | 10 +++++----- 11 files changed, 60 insertions(+), 10 deletions(-) diff --git a/include/robot_costmap_2d/critical_layer.h b/include/robot_costmap_2d/critical_layer.h index c31bd62..889245b 100644 --- a/include/robot_costmap_2d/critical_layer.h +++ b/include/robot_costmap_2d/critical_layer.h @@ -10,6 +10,10 @@ class CriticalLayer : public StaticLayer public: CriticalLayer(); virtual ~CriticalLayer(); + LayerType getType() const override + { + return LayerType::CRITICAL_LAYER; + } private: unsigned char interpretValue(unsigned char value) override; void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override; diff --git a/include/robot_costmap_2d/directional_layer.h b/include/robot_costmap_2d/directional_layer.h index 86fac6c..0a58394 100644 --- a/include/robot_costmap_2d/directional_layer.h +++ b/include/robot_costmap_2d/directional_layer.h @@ -13,7 +13,10 @@ namespace robot_costmap_2d virtual ~DirectionalLayer(); bool laneFilter(const std::vector plan); void resetMap(); - + LayerType getType() const override + { + return LayerType::DIRECTIONAL_LAYER; + } private: void incomingMap(const robot_nav_msgs::OccupancyGrid &new_map); bool laneFilter(std::vector> new_map, const robot_nav_msgs::Path path); diff --git a/include/robot_costmap_2d/inflation_layer.h b/include/robot_costmap_2d/inflation_layer.h index 50eed86..4de942f 100755 --- a/include/robot_costmap_2d/inflation_layer.h +++ b/include/robot_costmap_2d/inflation_layer.h @@ -67,6 +67,7 @@ public: unsigned int index_; unsigned int x_, y_; unsigned int src_x_, src_y_; + }; class InflationLayer : public Layer @@ -120,6 +121,10 @@ public: */ void setInflationParameters(double inflation_radius, double cost_scaling_factor); + LayerType getType() const override + { + return LayerType::INFLATION_LAYER; + } protected: virtual void onFootprintChanged(); boost::recursive_mutex* inflation_access_; diff --git a/include/robot_costmap_2d/layer.h b/include/robot_costmap_2d/layer.h index bcff269..f44610f 100755 --- a/include/robot_costmap_2d/layer.h +++ b/include/robot_costmap_2d/layer.h @@ -45,6 +45,21 @@ #include namespace robot_costmap_2d { +enum class LayerType +{ + UNKNOWN, + STATIC_LAYER, + OBSTACLE_LAYER, + INFLATION_LAYER, + CRITICAL_LAYER, + DIRECTIONAL_LAYER, + PREFERRED_LAYER, + UNPREFERRED_LAYER, + VOXEL_LAYER +}; + + + class LayeredCostmap; class Layer @@ -79,7 +94,7 @@ public: virtual void reset() {} - virtual ~Layer() {} + virtual ~Layer() = default; /** * @brief Check to make sure all the data in the layer is up to date. @@ -130,6 +145,8 @@ public: * notified of changes to the robot's footprint. */ virtual void onFootprintChanged() {} + virtual LayerType getType() const { return LayerType::UNKNOWN; } + template void dataCallBack(const T& value, const std::string& topic) { handle(value, topic); diff --git a/include/robot_costmap_2d/obstacle_layer.h b/include/robot_costmap_2d/obstacle_layer.h index 1db6bab..c236b81 100755 --- a/include/robot_costmap_2d/obstacle_layer.h +++ b/include/robot_costmap_2d/obstacle_layer.h @@ -76,6 +76,11 @@ public: void addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing); void clearStaticObservations(bool marking, bool clearing); + LayerType getType() const override + { + return LayerType::OBSTACLE_LAYER; + } + protected: void handleImpl(const void* data, const std::type_info&, diff --git a/include/robot_costmap_2d/preferred_layer.h b/include/robot_costmap_2d/preferred_layer.h index e1252af..f2acc24 100644 --- a/include/robot_costmap_2d/preferred_layer.h +++ b/include/robot_costmap_2d/preferred_layer.h @@ -10,6 +10,10 @@ class PreferredLayer : public StaticLayer public: PreferredLayer(); virtual ~PreferredLayer(); + LayerType getType() const override + { + return LayerType::PREFERRED_LAYER; + } private: unsigned char interpretValue(unsigned char value); }; diff --git a/include/robot_costmap_2d/static_layer.h b/include/robot_costmap_2d/static_layer.h index 3df4a59..6e6c466 100755 --- a/include/robot_costmap_2d/static_layer.h +++ b/include/robot_costmap_2d/static_layer.h @@ -63,6 +63,12 @@ public: virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); virtual void matchSize(); + + LayerType getType() const override + { + return LayerType::STATIC_LAYER; + } + protected: void handleImpl(const void* data, const std::type_info& type, diff --git a/include/robot_costmap_2d/unpreferred_layer.h b/include/robot_costmap_2d/unpreferred_layer.h index 1da5a86..f81089e 100644 --- a/include/robot_costmap_2d/unpreferred_layer.h +++ b/include/robot_costmap_2d/unpreferred_layer.h @@ -10,7 +10,10 @@ class UnPreferredLayer : public StaticLayer public: UnPreferredLayer(); virtual ~UnPreferredLayer(); - + LayerType getType() const override + { + return LayerType::UNPREFERRED_LAYER; + } private: unsigned char interpretValue(unsigned char value); diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index 3bdcf8c..6ea7293 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -76,7 +76,10 @@ public: } virtual void matchSize(); virtual void reset(); - + LayerType getType() const override + { + return LayerType::VOXEL_LAYER; + } protected: diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index b522936..06f82a6 100755 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -347,7 +347,7 @@ void StaticLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_ tf3::TransformStampedMsg transformMsg; try { - transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time::now()); + transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time()); } catch (tf3::TransformException ex) { diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index a6fbcad..c73b86c 100755 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -66,7 +66,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) std::string tf_error; robot_geometry_msgs::TransformStamped transformStamped; - if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error)) + if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time(), &tf_error)) { 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()); @@ -90,7 +90,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform( new_global_frame, // frame đích origin.header.frame_id, // frame nguồn - transform_time + tf3::Time() ); tf3::doTransform(origin, origin, tfm_1); obs.origin_ = origin.point; @@ -100,7 +100,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform( new_global_frame, // frame đích obs.cloud_->header.frame_id, // frame nguồn - transform_time + tf3::Time() ); tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2); } @@ -140,7 +140,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud) tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform( global_frame_, // frame đích local_origin.header.frame_id, // frame nguồn - data_convert::convertTime(local_origin.header.stamp) + tf3::Time() ); tf3::doTransform(local_origin, global_origin, tfm_1); @@ -161,7 +161,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud) tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform( global_frame_, // frame đích cloud.header.frame_id, // frame nguồn - data_convert::convertTime(cloud.header.stamp) + tf3::Time() ); tf3::doTransform(cloud, global_frame_cloud, tfm_2); global_frame_cloud.header.stamp = cloud.header.stamp;