Skip to content
Snippets Groups Projects
Unverified Commit 13e01d2d authored by Alexey Merzlyakov's avatar Alexey Merzlyakov Committed by GitHub
Browse files

Fix Map Saver and testing: (#2084)

- Add Map Saver resource sharing
- Add forgotten reset of save_map_service_
- Remove unnecessary map republisher
- Increase save_map_timeout
parent 8ba925cb
No related branches found
No related tags found
No related merge requests found
......@@ -35,6 +35,7 @@
#include <memory>
#include <stdexcept>
#include <functional>
#include <mutex>
using namespace std::placeholders;
......@@ -99,6 +100,9 @@ nav2_util::CallbackReturn
MapSaver::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(get_logger(), "Cleaning up");
save_map_service_.reset();
return nav2_util::CallbackReturn::SUCCESS;
}
......@@ -148,6 +152,9 @@ bool MapSaver::saveMapTopicToFile(
// Pointer to map message received in the subscription callback
nav_msgs::msg::OccupancyGrid::SharedPtr map_msg = nullptr;
// Mutex for handling map_msg shared resource
std::recursive_mutex access;
// Correct map_topic_loc if necessary
if (map_topic_loc == "") {
map_topic_loc = "map";
......@@ -173,8 +180,9 @@ bool MapSaver::saveMapTopicToFile(
}
// A callback function that receives map message from subscribed topic
auto mapCallback = [&map_msg](
auto mapCallback = [&map_msg, &access](
const nav_msgs::msg::OccupancyGrid::SharedPtr msg) -> void {
std::lock_guard<std::recursive_mutex> guard(access);
map_msg = msg;
};
......@@ -198,6 +206,9 @@ bool MapSaver::saveMapTopicToFile(
}
if (map_msg) {
std::lock_guard<std::recursive_mutex> guard(access);
// map_sub is no more needed
map_sub.reset();
// Map message received. Saving it to file
if (saveMapToFile(*map_msg, save_parameters_loc)) {
RCLCPP_INFO(get_logger(), "Map saved successfully");
......
......@@ -15,7 +15,6 @@
#include <experimental/filesystem>
#include <string>
#include <memory>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "nav2_map_server/map_io.hpp"
......@@ -23,7 +22,6 @@
#define TEST_DIR TEST_DIRECTORY
using namespace std::chrono_literals;
using namespace nav2_map_server; // NOLINT
using std::experimental::filesystem::path;
......@@ -34,7 +32,8 @@ public:
: Node("map_publisher")
{
std::string pub_map_file = path(TEST_DIR) / path(g_valid_yaml_file);
LOAD_MAP_STATUS status = loadMapFromYaml(pub_map_file, msg_);
nav_msgs::msg::OccupancyGrid msg;
LOAD_MAP_STATUS status = loadMapFromYaml(pub_map_file, msg);
if (status != LOAD_MAP_SUCCESS) {
RCLCPP_ERROR(get_logger(), "Can not load %s map file", pub_map_file);
return;
......@@ -43,19 +42,11 @@ public:
map_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>(
"map",
rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
timer_ = create_wall_timer(300ms, std::bind(&TestPublisher::mapPublishCallback, this));
map_pub_->publish(msg);
}
protected:
void mapPublishCallback()
{
map_pub_->publish(msg_);
}
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
rclcpp::TimerBase::SharedPtr timer_;
nav_msgs::msg::OccupancyGrid msg_;
};
int main(int argc, char ** argv)
......
map_saver:
ros__parameters:
save_map_timeout: 1000
save_map_timeout: 5000
free_thresh_default: 0.196
occupied_thresh_default: 0.65
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment