diff --git a/include/grid_map_geo/grid_map_geo.hpp b/include/grid_map_geo/grid_map_geo.hpp index d25e115..29d91a3 100644 --- a/include/grid_map_geo/grid_map_geo.hpp +++ b/include/grid_map_geo/grid_map_geo.hpp @@ -48,7 +48,7 @@ struct Location { class GridMapGeo { public: - GridMapGeo(const std::string frame_id = "map"); + GridMapGeo(const std::string& frame_id = "map"); virtual ~GridMapGeo(); /** @@ -91,6 +91,16 @@ class GridMapGeo { */ std::string getCoordinateName() { return coordinate_name_; }; + + /** + * @brief Overloading terrain loading with only elevation + * + * @param map_path Path to dsm path (Supported formats are *.tif) + */ + bool Load(const std::string& map_path) { + Load(map_path, ""); + } + /** * @brief Helper function for loading terrain from path * @@ -99,7 +109,7 @@ class GridMapGeo { * @return true Successfully loaded terrain * @return false Failed to load terrain */ - bool Load(const std::string& map_path, const std::string color_map_path = ""); + bool Load(const std::string& map_path, const std::string &color_map_path); /** * @brief Initialize grid map from a geotiff file @@ -170,7 +180,7 @@ class GridMapGeo { */ void AddLayerNormals(std::string reference_layer); - geometry_msgs::msg::TransformStamped static_transformStamped; + geometry_msgs::msg::TransformStamped static_transformStamped_; protected: grid_map::GridMap grid_map_; diff --git a/launch/load_multiple_tif_launch.xml b/launch/load_multiple_tif_launch.xml index 055feb9..1b31076 100644 --- a/launch/load_multiple_tif_launch.xml +++ b/launch/load_multiple_tif_launch.xml @@ -1,6 +1,5 @@ - diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp index debe96f..a0d8249 100644 --- a/src/grid_map_geo.cpp +++ b/src/grid_map_geo.cpp @@ -58,11 +58,11 @@ #include #endif -GridMapGeo::GridMapGeo(const std::string frame_id) { frame_id_ = frame_id; } +GridMapGeo::GridMapGeo(const std::string& frame_id) { frame_id_ = frame_id; } GridMapGeo::~GridMapGeo() {} -bool GridMapGeo::Load(const std::string &map_path, const std::string color_map_path) { +bool GridMapGeo::Load(const std::string &map_path, const std::string &color_map_path) { bool loaded = initializeFromGeotiff(map_path); if (!color_map_path.empty()) { // Load color layer if the color path is nonempty bool color_loaded = addColorFromGeotiff(color_map_path); diff --git a/src/test_tif_loader.cpp b/src/test_tif_loader.cpp index e3f64fc..d139ea2 100644 --- a/src/test_tif_loader.cpp +++ b/src/test_tif_loader.cpp @@ -78,18 +78,18 @@ class MapPublisher : public rclcpp::Node { Eigen::Vector3d map_origin; map_->getGlobalOrigin(epsg, map_origin); - geometry_msgs::msg::TransformStamped static_transformStamped; - static_transformStamped.header.frame_id = map_->getCoordinateName(); - static_transformStamped.child_frame_id = map_->getGridMap().getFrameId(); - static_transformStamped.transform.translation.x = map_origin(0); - static_transformStamped.transform.translation.y = map_origin(1); - static_transformStamped.transform.translation.z = 0.0; - static_transformStamped.transform.rotation.x = 0.0; - static_transformStamped.transform.rotation.y = 0.0; - static_transformStamped.transform.rotation.z = 0.0; - static_transformStamped.transform.rotation.w = 1.0; + geometry_msgs::msg::TransformStamped static_transformStamped_; + static_transformStamped_.header.frame_id = map_->getCoordinateName(); + static_transformStamped_.child_frame_id = map_->getGridMap().getFrameId(); + static_transformStamped_.transform.translation.x = map_origin.x(); + static_transformStamped_.transform.translation.y = map_origin.y(); + static_transformStamped_.transform.translation.z = 0.0; + static_transformStamped_.transform.rotation.x = 0.0; + static_transformStamped_.transform.rotation.y = 0.0; + static_transformStamped_.transform.rotation.z = 0.0; + static_transformStamped_.transform.rotation.w = 1.0; - tf_broadcaster_->sendTransform(static_transformStamped); + tf_broadcaster_->sendTransform(static_transformStamped_); } private: