From b6733ae04cc2ec79409faf05bbae5f70a3c7fcd2 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Wed, 31 Dec 2025 15:43:07 +0700 Subject: [PATCH] update --- include/robot_costmap_2d/costmap_2d_robot.h | 10 +++++----- include/robot_costmap_2d/critical_layer.h | 4 ---- include/robot_costmap_2d/directional_layer.h | 5 +---- include/robot_costmap_2d/footprint.h | 2 +- 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/footprint.cpp | 16 ++++++++-------- src/observation_buffer.cpp | 10 +++++----- 14 files changed, 24 insertions(+), 74 deletions(-) diff --git a/include/robot_costmap_2d/costmap_2d_robot.h b/include/robot_costmap_2d/costmap_2d_robot.h index de643b5..c37e258 100755 --- a/include/robot_costmap_2d/costmap_2d_robot.h +++ b/include/robot_costmap_2d/costmap_2d_robot.h @@ -55,18 +55,18 @@ #include #include -class SuperValue : public robot::XmlRpc::XmlRpcValue +class SuperValue : public robot_xmlrpcpp::XmlRpcValue { public: - void setStruct(robot::XmlRpc::XmlRpcValue::ValueStruct* a) + void setStruct(robot_xmlrpcpp::XmlRpcValue::ValueStruct* a) { _type = TypeStruct; - _value.asStruct = new robot::XmlRpc::XmlRpcValue::ValueStruct(*a); + _value.asStruct = new robot_xmlrpcpp::XmlRpcValue::ValueStruct(*a); } - void setArray(robot::XmlRpc::XmlRpcValue::ValueArray* a) + void setArray(robot_xmlrpcpp::XmlRpcValue::ValueArray* a) { _type = TypeArray; - _value.asArray = new std::vector(*a); + _value.asArray = new std::vector(*a); } }; diff --git a/include/robot_costmap_2d/critical_layer.h b/include/robot_costmap_2d/critical_layer.h index 889245b..c31bd62 100644 --- a/include/robot_costmap_2d/critical_layer.h +++ b/include/robot_costmap_2d/critical_layer.h @@ -10,10 +10,6 @@ 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 0a58394..86fac6c 100644 --- a/include/robot_costmap_2d/directional_layer.h +++ b/include/robot_costmap_2d/directional_layer.h @@ -13,10 +13,7 @@ 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/footprint.h b/include/robot_costmap_2d/footprint.h index 83e3553..de44a3f 100755 --- a/include/robot_costmap_2d/footprint.h +++ b/include/robot_costmap_2d/footprint.h @@ -136,7 +136,7 @@ std::vector makeFootprintFromParams(robot::NodeHandl * @param full_param_name this is the full name of the rosparam from * which the footprint_xmlrpc value came. It is used only for * reporting errors. */ -std::vector makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc, +std::vector makeFootprintFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name); // /** @brief Write the current unpadded_footprint_ to the "footprint" diff --git a/include/robot_costmap_2d/inflation_layer.h b/include/robot_costmap_2d/inflation_layer.h index 4de942f..50eed86 100755 --- a/include/robot_costmap_2d/inflation_layer.h +++ b/include/robot_costmap_2d/inflation_layer.h @@ -67,7 +67,6 @@ public: unsigned int index_; unsigned int x_, y_; unsigned int src_x_, src_y_; - }; class InflationLayer : public Layer @@ -121,10 +120,6 @@ 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 f44610f..bcff269 100755 --- a/include/robot_costmap_2d/layer.h +++ b/include/robot_costmap_2d/layer.h @@ -45,21 +45,6 @@ #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 @@ -94,7 +79,7 @@ public: virtual void reset() {} - virtual ~Layer() = default; + virtual ~Layer() {} /** * @brief Check to make sure all the data in the layer is up to date. @@ -145,8 +130,6 @@ 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 c236b81..1db6bab 100755 --- a/include/robot_costmap_2d/obstacle_layer.h +++ b/include/robot_costmap_2d/obstacle_layer.h @@ -76,11 +76,6 @@ 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 f2acc24..e1252af 100644 --- a/include/robot_costmap_2d/preferred_layer.h +++ b/include/robot_costmap_2d/preferred_layer.h @@ -10,10 +10,6 @@ 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 6e6c466..3df4a59 100755 --- a/include/robot_costmap_2d/static_layer.h +++ b/include/robot_costmap_2d/static_layer.h @@ -63,12 +63,6 @@ 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 f81089e..1da5a86 100644 --- a/include/robot_costmap_2d/unpreferred_layer.h +++ b/include/robot_costmap_2d/unpreferred_layer.h @@ -10,10 +10,7 @@ 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 6ea7293..3bdcf8c 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -76,10 +76,7 @@ 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 06f82a6..b522936 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()); + transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time::now()); } catch (tf3::TransformException ex) { diff --git a/src/footprint.cpp b/src/footprint.cpp index bbc2983..dab07dd 100755 --- a/src/footprint.cpp +++ b/src/footprint.cpp @@ -249,25 +249,25 @@ std::vector makeFootprintFromParams(robot::NodeHandl // nh.setParam("footprint", oss.str().c_str()); // } -double getNumberFromXMLRPC(robot::XmlRpc::XmlRpcValue& value, const std::string& full_param_name) +double getNumberFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& value, const std::string& full_param_name) { // Make sure that the value we're looking at is either a double or an int. - if (value.getType() != robot::XmlRpc::XmlRpcValue::TypeInt && - value.getType() != robot::XmlRpc::XmlRpcValue::TypeDouble) + if (value.getType() != robot_xmlrpcpp::XmlRpcValue::TypeInt && + value.getType() != robot_xmlrpcpp::XmlRpcValue::TypeDouble) { std::string& value_string = value; printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n", full_param_name.c_str(), value_string.c_str()); throw std::runtime_error("Values in the footprint specification must be numbers"); } - return value.getType() == robot::XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value); + return value.getType() == robot_xmlrpcpp::XmlRpcValue::TypeInt ? (int)(value) : (double)(value); } -std::vector makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc, +std::vector makeFootprintFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name) { // Make sure we have an array of at least 3 elements. - if (footprint_xmlrpc.getType() != robot::XmlRpc::XmlRpcValue::TypeArray || + if (footprint_xmlrpc.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray || footprint_xmlrpc.size() < 3) { printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n", @@ -282,8 +282,8 @@ std::vector makeFootprintFromXMLRPC(robot::XmlRpc::X for (int i = 0; i < footprint_xmlrpc.size(); ++i) { // Make sure each element of the list is an array of size 2. (x and y coordinates) - robot::XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ]; - if (point.getType() != robot::XmlRpc::XmlRpcValue::TypeArray || + robot_xmlrpcpp::XmlRpcValue point = footprint_xmlrpc[ i ]; + if (point.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray || point.size() != 2) { printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: " diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index c73b86c..a6fbcad 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_, tf3::Time(), &tf_error)) + if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_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 - tf3::Time() + transform_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 - tf3::Time() + transform_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 - tf3::Time() + data_convert::convertTime(local_origin.header.stamp) ); 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 - tf3::Time() + data_convert::convertTime(cloud.header.stamp) ); tf3::doTransform(cloud, global_frame_cloud, tfm_2); global_frame_cloud.header.stamp = cloud.header.stamp;