diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 9ca53156419cb58390dd97ff0d59e36ded0d3ad3..17c5b6601426300935fa8930c65d30708bfe867a 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(SDL_image REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) +find_package(nav2_lifecycle REQUIRED) find_package(nav2_util REQUIRED) nav2_package() @@ -47,6 +48,7 @@ set(map_server_dependencies std_msgs tf2 nav2_util + nav2_lifecycle ) set(map_saver_dependencies diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp index fdb178aeb1f6bf5e2a70543a54a32dac8b1a8ba8..1f3d575398d4661f9d555f51ff39942645bfa05b 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -15,38 +15,30 @@ #ifndef NAV2_MAP_SERVER__MAP_SERVER_HPP_ #define NAV2_MAP_SERVER__MAP_SERVER_HPP_ -#include <string> #include <memory> -#include <vector> -#include "rclcpp/rclcpp.hpp" -#include "nav2_map_server/map_loader.hpp" -#include "yaml-cpp/yaml.h" +#include "nav2_lifecycle/lifecycle_node.hpp" +#include "nav2_lifecycle/lifecycle_helper_interface.hpp" namespace nav2_map_server { -class MapServer : public rclcpp::Node +class MapServer : public nav2_lifecycle::LifecycleNode { public: - explicit MapServer(const std::string & node_name); MapServer(); - -private: - void getParameters(); - - // The map server has one node parameter, the YAML filename - std::string yaml_filename_; - - // The YAML document from which to get the conversion parameters - YAML::Node doc_; - - // The map type ("occupancy") from the YAML document which specifies - // the kind of loader to create - std::string map_type_; - std::unique_ptr<MapLoader> map_loader_; - - // The map filename ("image") from the YAML document to pass to the map loader - std::string map_filename_; + ~MapServer(); + +protected: + // Implement the lifecycle interface + nav2_lifecycle::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + nav2_lifecycle::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + nav2_lifecycle::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + nav2_lifecycle::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + nav2_lifecycle::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + nav2_lifecycle::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; + + // The map loader that will actually do the work + std::unique_ptr<nav2_lifecycle::LifecycleHelperInterface> map_loader_; }; } // namespace nav2_map_server diff --git a/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp b/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp index e2909ca5dd8e857ba6a92aa2a9db1c8037058b9c..0f7c81a5736863b722ee481b906b229f16c472d1 100644 --- a/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp +++ b/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp @@ -15,68 +15,68 @@ #ifndef NAV2_MAP_SERVER__OCC_GRID_LOADER_HPP_ #define NAV2_MAP_SERVER__OCC_GRID_LOADER_HPP_ +#include <memory> #include <string> #include <vector> #include "rclcpp/rclcpp.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" #include "nav_msgs/srv/get_map.hpp" -#include "nav2_map_server/map_loader.hpp" -#include "yaml-cpp/yaml.h" +#include "nav2_lifecycle/lifecycle_node.hpp" namespace nav2_map_server { -class OccGridLoader : public MapLoader +class OccGridLoader : public nav2_lifecycle::LifecycleHelperInterface { public: - explicit OccGridLoader(rclcpp::Node * node, YAML::Node & doc); + OccGridLoader(rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & yaml_filename); OccGridLoader() = delete; + ~OccGridLoader(); - // Load the image and generate an OccupancyGrid - void loadMapFromFile(const std::string & filename) override; - - // Make the OccupancyGrid available via ROS topic and service - void startServices() override; + nav2_lifecycle::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + nav2_lifecycle::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + nav2_lifecycle::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + nav2_lifecycle::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; protected: // The ROS node to use for ROS-related operations such as creating a service - rclcpp::Node * node_; - - // The YAML file from which to get the conversion parameters - YAML::Node & doc_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + + // The name of the YAML file from which to get the conversion parameters + std::string yaml_filename_; + + typedef enum { TRINARY, SCALE, RAW } MapMode; + typedef struct + { + double resolution{0}; + std::vector<double> origin{0, 0, 0}; + double free_thresh{0}; + double occupied_thresh{0}; + MapMode mode{TRINARY}; + int negate{0}; + } LoadParameters; - // Conversion parameters read from the YAML document - double resolution_; - std::vector<double> origin_; - double free_thresh_; - double occupied_thresh_; - enum MapMode { TRINARY, SCALE, RAW }; - MapMode mode_; - int negate_; + // Load the image and generate an OccupancyGrid + void loadMapFromFile(const std::string & filename, LoadParameters * loadParameters); - // A service to provide the ouccpancy grid (GetMap) and the message to return + // A service to provide the occupancy grid (GetMap) and the message to return rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr occ_service_; // A topic on which the occupancy grid will be published - rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_pub_; + rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_pub_; // The message to publish on the occupancy grid topic - nav_msgs::msg::OccupancyGrid msg_; + std::unique_ptr<nav_msgs::msg::OccupancyGrid> msg_; // The frame ID used in the returned OccupancyGrid message - static const char * frame_id_; + static constexpr const char * frame_id_{"map"}; // The name for the topic on which the map will be published - static const char * topic_name_; + static constexpr const char * topic_name_{"map"}; // The name of the service for getting a map - static const char * service_name_; - - // For now, use a timer to publish the map periodically so that it is sure - // to be received on the ROS1 side across the ROS1 bridge - rclcpp::TimerBase::SharedPtr timer_; + static constexpr const char * service_name_{"map"}; }; } // namespace nav2_map_server diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 02972a53d5181debbb0c4c5302d234f6f7bb2499..5e13985925a2d5cef2ed9f9d764aab58daf35961 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -10,7 +10,6 @@ <license>Apache License 2.0</license> <buildtool_depend>ament_cmake</buildtool_depend> - <build_depend>nav2_common</build_depend> <depend>nav_msgs</depend> <depend>std_msgs</depend> @@ -22,6 +21,7 @@ <depend>launch_testing</depend> <depend>tf2</depend> <depend>nav2_util</depend> + <depend>nav2_lifecycle</depend> <test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_auto</test_depend> diff --git a/nav2_map_server/src/main.cpp b/nav2_map_server/src/main.cpp index 7bac49e9c17cf0bf7d4b37d1b58915308bca8e9d..8e57cc09bf985d7e0a17e0749ec70c95e12f6066 100644 --- a/nav2_map_server/src/main.cpp +++ b/nav2_map_server/src/main.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include <string> #include <memory> #include <stdexcept> -#include "rclcpp/rclcpp.hpp" +#include <string> + #include "nav2_map_server/map_server.hpp" +#include "rclcpp/rclcpp.hpp" int main(int argc, char ** argv) { @@ -24,12 +25,13 @@ int main(int argc, char ** argv) try { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared<nav2_map_server::MapServer>(node_name)); + auto node = std::make_shared<nav2_map_server::MapServer>(); + rclcpp::spin(node->get_node_base_interface()); rclcpp::shutdown(); + return 0; } catch (std::exception & ex) { RCLCPP_ERROR(rclcpp::get_logger(node_name.c_str()), ex.what()); RCLCPP_ERROR(rclcpp::get_logger(node_name.c_str()), "Exiting"); + return -1; } - - return 0; } diff --git a/nav2_map_server/src/map_server.cpp b/nav2_map_server/src/map_server.cpp index 042e54f1eee750c06938217bf5eb3fb5cd6dc58f..80b8c9eddae3d7af65c6f45b7ae7a978e0705f7e 100644 --- a/nav2_map_server/src/map_server.cpp +++ b/nav2_map_server/src/map_server.cpp @@ -14,11 +14,11 @@ #include "nav2_map_server/map_server.hpp" -#include <libgen.h> -#include <string> -#include <memory> #include <fstream> +#include <memory> #include <stdexcept> +#include <string> + #include "nav2_map_server/occ_grid_loader.hpp" #include "nav2_util/node_utils.hpp" #include "yaml-cpp/yaml.h" @@ -28,66 +28,99 @@ using namespace std::chrono_literals; namespace nav2_map_server { -MapServer::MapServer(const std::string & node_name) -: Node(node_name, nav2_util::get_node_options_default()) +MapServer::MapServer() +: nav2_lifecycle::LifecycleNode("map_server") { - // Get the MAP YAML file, which includes the image filename and the map type - getParameters(); - - // Create the proper map loader for the specified map type - if (map_type_ == "occupancy") { - map_loader_ = std::make_unique<OccGridLoader>(this, doc_); - } else { - std::string msg = "Cannot load unknown map type: '" + map_type_ + "'"; - throw std::runtime_error(msg); - } + RCLCPP_INFO(get_logger(), "Creating"); - // Load the map and provide access via topic and service interfaces - map_loader_->loadMapFromFile(map_filename_); - map_loader_->startServices(); + // Declare the node parameters + declare_parameter("yaml_filename", rclcpp::ParameterValue(std::string("map.yaml"))); } -MapServer::MapServer() -: MapServer("map_server") +MapServer::~MapServer() { + RCLCPP_INFO(get_logger(), "Destroying"); } -void MapServer::getParameters() +nav2_lifecycle::CallbackReturn +MapServer::on_configure(const rclcpp_lifecycle::State & state) { - get_parameter_or_set("yaml_filename", yaml_filename_, std::string("map.yaml")); + RCLCPP_INFO(get_logger(), "Configuring"); + + // Get the name of the YAML file to use + std::string yaml_filename; + get_parameter("yaml_filename", yaml_filename); // Make sure that there's a valid file there and open it up - std::ifstream fin(yaml_filename_.c_str()); + std::ifstream fin(yaml_filename.c_str()); if (fin.fail()) { - throw std::runtime_error("Could not open '" + yaml_filename_ + "': file not found"); + throw std::runtime_error("Could not open '" + yaml_filename + "': file not found"); } - doc_ = YAML::LoadFile(yaml_filename_); - - // Get the name of the map file - try { - map_filename_ = doc_["image"].as<std::string>(); - if (map_filename_.size() == 0) { - throw std::runtime_error("The image tag cannot be an empty string"); - } - if (map_filename_[0] != '/') { - // dirname can modify what you pass it - char * fname_copy = strdup(yaml_filename_.c_str()); - map_filename_ = std::string(dirname(fname_copy)) + '/' + map_filename_; - free(fname_copy); - } - } catch (YAML::Exception) { - std::string msg = "'" + yaml_filename_ + "' does not contain an image tag or it is invalid"; - throw std::runtime_error(msg); - } + // The YAML document from which to get the conversion parameters + YAML::Node doc = YAML::LoadFile(yaml_filename); // Get the map type so that we can create the correct map loader + std::string map_type; try { - map_type_ = doc_["map_type"].as<std::string>(); + map_type = doc["map_type"].as<std::string>(); } catch (YAML::Exception) { // Default to occupancy grid if not specified in the YAML file - map_type_ = "occupancy"; + map_type = "occupancy"; + } + + // Create the correct map loader for the specified map type + if (map_type == "occupancy") { + map_loader_ = std::make_unique<OccGridLoader>(shared_from_this(), yaml_filename); + } else { + std::string msg = "Cannot load unknown map type: '" + map_type + "'"; + throw std::runtime_error(msg); } + + map_loader_->on_configure(state); + + return nav2_lifecycle::CallbackReturn::SUCCESS; +} + +nav2_lifecycle::CallbackReturn +MapServer::on_activate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Activating"); + map_loader_->on_activate(state); + return nav2_lifecycle::CallbackReturn::SUCCESS; +} + +nav2_lifecycle::CallbackReturn +MapServer::on_deactivate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + map_loader_->on_deactivate(state); + return nav2_lifecycle::CallbackReturn::SUCCESS; +} + +nav2_lifecycle::CallbackReturn +MapServer::on_cleanup(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + map_loader_->on_cleanup(state); + map_loader_.reset(); + + return nav2_lifecycle::CallbackReturn::SUCCESS; +} + +nav2_lifecycle::CallbackReturn +MapServer::on_error(const rclcpp_lifecycle::State &) +{ + RCLCPP_ERROR(get_logger(), "Handling error state"); + return nav2_lifecycle::CallbackReturn::SUCCESS; +} + +nav2_lifecycle::CallbackReturn +MapServer::on_shutdown(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_lifecycle::CallbackReturn::SUCCESS; } } // namespace nav2_map_server diff --git a/nav2_map_server/src/occ_grid_loader.cpp b/nav2_map_server/src/occ_grid_loader.cpp index ae32b636ac3cf137560f0ce720e98144391d3404..bf2e31271d6b66566023d9c489f6be9e16f6e13c 100644 --- a/nav2_map_server/src/occ_grid_loader.cpp +++ b/nav2_map_server/src/occ_grid_loader.cpp @@ -27,100 +27,195 @@ * POSSIBILITY OF SUCH DAMAGE. */ -/* - * This file contains helper functions for loading images as maps. - * - * Author: Brian Gerkey - */ +// This file contains helper functions for loading images as maps. +// Author: Brian Gerkey + #include "nav2_map_server/occ_grid_loader.hpp" -#include <string> -#include <vector> +#include <libgen.h> #include <memory> #include <stdexcept> +#include <string> +#include <vector> #include "tf2/LinearMath/Quaternion.h" #include "SDL/SDL_image.h" +#include "yaml-cpp/yaml.h" using namespace std::chrono_literals; namespace nav2_map_server { -const char * OccGridLoader::frame_id_ = "map"; -const char * OccGridLoader::topic_name_ = "map"; -const char * OccGridLoader::service_name_ = "map"; +OccGridLoader::OccGridLoader( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & yaml_filename) +: node_(node), yaml_filename_(yaml_filename) +{ + RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Creating"); +} -OccGridLoader::OccGridLoader(rclcpp::Node * node, YAML::Node & doc) -: node_(node), doc_(doc), origin_(3) +OccGridLoader::~OccGridLoader() { + RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Destroying"); +} + +nav2_lifecycle::CallbackReturn +OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Configuring"); + + msg_ = std::make_unique<nav_msgs::msg::OccupancyGrid>(); + + // The YAML document from which to get the conversion parameters + YAML::Node doc = YAML::LoadFile(yaml_filename_); + LoadParameters loadParameters; + + // Get the name of the map file + std::string map_filename; try { - resolution_ = doc_["resolution"].as<double>(); + map_filename = doc["image"].as<std::string>(); + if (map_filename.size() == 0) { + throw std::runtime_error("The image tag cannot be an empty string"); + } + if (map_filename[0] != '/') { + // dirname can modify what you pass it + char * fname_copy = strdup(yaml_filename_.c_str()); + map_filename = std::string(dirname(fname_copy)) + '/' + map_filename; + free(fname_copy); + } + } catch (YAML::Exception) { + throw std::runtime_error("The map does not contain an image tag or it is invalid"); + } + + try { + loadParameters.resolution = doc["resolution"].as<double>(); } catch (YAML::Exception) { throw std::runtime_error("The map does not contain a resolution tag or it is invalid"); } try { - origin_[0] = doc_["origin"][0].as<double>(); - origin_[1] = doc_["origin"][1].as<double>(); - origin_[2] = doc_["origin"][2].as<double>(); + loadParameters.origin[0] = doc["origin"][0].as<double>(); + loadParameters.origin[1] = doc["origin"][1].as<double>(); + loadParameters.origin[2] = doc["origin"][2].as<double>(); } catch (YAML::Exception) { throw std::runtime_error("The map does not contain an origin tag or it is invalid"); } try { - free_thresh_ = doc_["free_thresh"].as<double>(); + loadParameters.free_thresh = doc["free_thresh"].as<double>(); } catch (YAML::Exception) { throw std::runtime_error("The map does not contain a free_thresh tag or it is invalid"); } try { - occupied_thresh_ = doc_["occupied_thresh"].as<double>(); + loadParameters.occupied_thresh = doc["occupied_thresh"].as<double>(); } catch (YAML::Exception) { throw std::runtime_error("The map does not contain an occupied_thresh tag or it is invalid"); } std::string mode_str; try { - mode_str = doc_["mode"].as<std::string>(); + mode_str = doc["mode"].as<std::string>(); // Convert the string version of the mode name to one of the enumeration values if (mode_str == "trinary") { - mode_ = TRINARY; + loadParameters.mode = TRINARY; } else if (mode_str == "scale") { - mode_ = SCALE; + loadParameters.mode = SCALE; } else if (mode_str == "raw") { - mode_ = RAW; + loadParameters.mode = RAW; } else { RCLCPP_WARN(node_->get_logger(), "Mode parameter not recognized: '%s', using default value (trinary)", mode_str.c_str()); - mode_ = TRINARY; + loadParameters.mode = TRINARY; } } catch (YAML::Exception &) { RCLCPP_WARN(node_->get_logger(), "Mode parameter not set, using default value (trinary)"); - mode_ = TRINARY; + loadParameters.mode = TRINARY; } try { - negate_ = doc_["negate"].as<int>(); + loadParameters.negate = doc["negate"].as<int>(); } catch (YAML::Exception) { throw std::runtime_error("The map does not contain a negate tag or it is invalid"); } - RCLCPP_DEBUG(node_->get_logger(), "resolution: %f", resolution_); - RCLCPP_DEBUG(node_->get_logger(), "origin[0]: %f", origin_[0]); - RCLCPP_DEBUG(node_->get_logger(), "origin[1]: %f", origin_[1]); - RCLCPP_DEBUG(node_->get_logger(), "origin[2]: %f", origin_[2]); - RCLCPP_DEBUG(node_->get_logger(), "free_thresh: %f", free_thresh_); - RCLCPP_DEBUG(node_->get_logger(), "occupied_thresh: %f", occupied_thresh_); + RCLCPP_DEBUG(node_->get_logger(), "resolution: %f", loadParameters.resolution); + RCLCPP_DEBUG(node_->get_logger(), "origin[0]: %f", loadParameters.origin[0]); + RCLCPP_DEBUG(node_->get_logger(), "origin[1]: %f", loadParameters.origin[1]); + RCLCPP_DEBUG(node_->get_logger(), "origin[2]: %f", loadParameters.origin[2]); + RCLCPP_DEBUG(node_->get_logger(), "free_thresh: %f", loadParameters.free_thresh); + RCLCPP_DEBUG(node_->get_logger(), "occupied_thresh: %f", loadParameters.occupied_thresh); RCLCPP_DEBUG(node_->get_logger(), "mode_str: %s", mode_str.c_str()); - RCLCPP_DEBUG(node_->get_logger(), "mode: %d", mode_); - RCLCPP_DEBUG(node_->get_logger(), "negate: %d", negate_); + RCLCPP_DEBUG(node_->get_logger(), "mode: %d", loadParameters.mode); + RCLCPP_DEBUG(node_->get_logger(), "negate: %d", loadParameters.negate); + + loadMapFromFile(map_filename, &loadParameters); + + // Create a service callback handle + auto handle_occ_callback = [this]( + const std::shared_ptr<rmw_request_id_t>/*request_header*/, + const std::shared_ptr<nav_msgs::srv::GetMap::Request>/*request*/, + std::shared_ptr<nav_msgs::srv::GetMap::Response> response) -> void { + RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling map request"); + response->map = *msg_; + }; + + // Create a service that provides the occupancy grid + occ_service_ = node_->create_service<nav_msgs::srv::GetMap>(service_name_, handle_occ_callback); + + // Create a publisher using the QoS settings to emulate a ROS1 latched topic + rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; + custom_qos_profile.depth = 1; + custom_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + custom_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + occ_pub_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>( + topic_name_, custom_qos_profile); + + return nav2_lifecycle::CallbackReturn::SUCCESS; +} + +nav2_lifecycle::CallbackReturn +OccGridLoader::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Activating"); + + // Publish the map using the latched topic + occ_pub_->on_activate(); + occ_pub_->publish(*msg_); + + return nav2_lifecycle::CallbackReturn::SUCCESS; +} + +nav2_lifecycle::CallbackReturn +OccGridLoader::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Deactivating"); + + occ_pub_->on_deactivate(); + + return nav2_lifecycle::CallbackReturn::SUCCESS; +} + +nav2_lifecycle::CallbackReturn +OccGridLoader::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Cleaning up"); + + occ_pub_.reset(); + occ_service_.reset(); + msg_.reset(); + + return nav2_lifecycle::CallbackReturn::SUCCESS; } -void OccGridLoader::loadMapFromFile(const std::string & map_name) +void +OccGridLoader::loadMapFromFile(const std::string & map_name, LoadParameters * loadParameters) { + RCLCPP_DEBUG(node_->get_logger(), "OccGridLoader: loadMapFromFile"); + // Load the image using SDL. If we get NULL back, the image load failed. SDL_Surface * img; if (!(img = IMG_Load(map_name.c_str()))) { @@ -130,21 +225,21 @@ void OccGridLoader::loadMapFromFile(const std::string & map_name) } // Copy the image data into the map structure - msg_.info.width = img->w; - msg_.info.height = img->h; - msg_.info.resolution = resolution_; - msg_.info.origin.position.x = origin_[0]; - msg_.info.origin.position.y = origin_[1]; - msg_.info.origin.position.z = 0.0; + msg_->info.width = img->w; + msg_->info.height = img->h; + msg_->info.resolution = loadParameters->resolution; + msg_->info.origin.position.x = loadParameters->origin[0]; + msg_->info.origin.position.y = loadParameters->origin[1]; + msg_->info.origin.position.z = 0.0; tf2::Quaternion q; - q.setRPY(0, 0, origin_[2]); - msg_.info.origin.orientation.x = q.x(); - msg_.info.origin.orientation.y = q.y(); - msg_.info.origin.orientation.z = q.z(); - msg_.info.origin.orientation.w = q.w(); + q.setRPY(0, 0, loadParameters->origin[2]); + msg_->info.origin.orientation.x = q.x(); + msg_->info.origin.orientation.y = q.y(); + msg_->info.origin.orientation.z = q.z(); + msg_->info.origin.orientation.w = q.w(); // Allocate space to hold the data - msg_.data.resize(msg_.info.width * msg_.info.height); + msg_->data.resize(msg_->info.width * msg_->info.height); // Get values that we'll need to iterate through the pixels int rowstride = img->pitch; @@ -153,7 +248,7 @@ void OccGridLoader::loadMapFromFile(const std::string & map_name) // NOTE: Trinary mode still overrides here to preserve existing behavior. // Alpha will be averaged in with color channels when using trinary mode. int avg_channels; - if (mode_ == TRINARY || !img->format->Amask) { + if (loadParameters->mode == TRINARY || !img->format->Amask) { avg_channels = n_channels; } else { avg_channels = n_channels - 1; @@ -162,8 +257,8 @@ void OccGridLoader::loadMapFromFile(const std::string & map_name) // Copy pixel data into the map structure unsigned char * pixels = (unsigned char *)(img->pixels); int color_sum; - for (unsigned int j = 0; j < msg_.info.height; j++) { - for (unsigned int i = 0; i < msg_.info.width; i++) { + for (unsigned int j = 0; j < msg_->info.height; j++) { + for (unsigned int i = 0; i < msg_->info.width; i++) { // Compute mean of RGB for this pixel unsigned char * p = pixels + j * rowstride + i * n_channels; color_sum = 0; @@ -179,16 +274,16 @@ void OccGridLoader::loadMapFromFile(const std::string & map_name) alpha = *(p + n_channels - 1); } - if (negate_) { + if (loadParameters->negate) { color_avg = 255 - color_avg; } #define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) unsigned char value; - if (mode_ == RAW) { + if (loadParameters->mode == RAW) { value = color_avg; - msg_.data[MAP_IDX(msg_.info.width, i, msg_.info.height - j - 1)] = value; + msg_->data[MAP_IDX(msg_->info.width, i, msg_->info.height - j - 1)] = value; continue; } @@ -199,65 +294,33 @@ void OccGridLoader::loadMapFromFile(const std::string & map_name) // Apply thresholds to RGB means to determine occupancy values for // map. Note that we invert the graphics-ordering of the pixels to // produce a map with cell (0,0) in the lower-left corner. - if (occ > occupied_thresh_) { + if (occ > loadParameters->occupied_thresh) { value = +100; - } else if (occ < free_thresh_) { + } else if (occ < loadParameters->free_thresh) { value = 0; - } else if (mode_ == TRINARY || alpha < 1.0) { + } else if (loadParameters->mode == TRINARY || alpha < 1.0) { value = -1; } else { - double ratio = (occ - free_thresh_) / (occupied_thresh_ - free_thresh_); + double ratio = (occ - loadParameters->free_thresh) / + (loadParameters->occupied_thresh - loadParameters->free_thresh); value = 99 * ratio; } - msg_.data[MAP_IDX(msg_.info.width, i, msg_.info.height - j - 1)] = value; + msg_->data[MAP_IDX(msg_->info.width, i, msg_->info.height - j - 1)] = value; } } SDL_FreeSurface(img); - msg_.info.map_load_time = node_->now(); - msg_.header.frame_id = frame_id_; - msg_.header.stamp = node_->now(); + msg_->info.map_load_time = node_->now(); + msg_->header.frame_id = frame_id_; + msg_->header.stamp = node_->now(); RCLCPP_DEBUG(node_->get_logger(), "Read map %s: %d X %d map @ %.3lf m/cell", map_name.c_str(), - msg_.info.width, - msg_.info.height, - msg_.info.resolution); -} - -void OccGridLoader::startServices() -{ - // Create a service callback handle - auto handle_occ_callback = [this]( - const std::shared_ptr<rmw_request_id_t>/*request_header*/, - const std::shared_ptr<nav_msgs::srv::GetMap::Request>/*request*/, - std::shared_ptr<nav_msgs::srv::GetMap::Response> response) -> void { - RCLCPP_INFO(node_->get_logger(), "Handling map request"); - response->map = msg_; - }; - - // Create a service that provides the occupancy grid - occ_service_ = node_->create_service<nav_msgs::srv::GetMap>(service_name_, handle_occ_callback); - - // Create a publisher using the QoS settings to emulate a ROS1 latched topic - rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; - custom_qos_profile.depth = 1; - custom_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - custom_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - occ_pub_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>( - topic_name_, custom_qos_profile); - - // Publish the map using the latched topic - occ_pub_->publish(msg_); - - // TODO(mjeronimo): Remove the following once we've got everything on the ROS2 side - // - // Periodically publish the map so that the ros1 bridge will be sure the proxy the - // message to rviz on the ROS1 side - auto timer_callback = [this]() -> void {occ_pub_->publish(msg_);}; - timer_ = node_->create_wall_timer(2s, timer_callback); + msg_->info.width, + msg_->info.height, + msg_->info.resolution); } } // namespace nav2_map_server diff --git a/nav2_map_server/test/component/test_occ_grid_node.cpp b/nav2_map_server/test/component/test_occ_grid_node.cpp index 31228ff74b3f3f040628d0b4e4c338a4909f8864..fc7197622c6dc97e0c3cc5e535325a3628929f7e 100644 --- a/nav2_map_server/test/component/test_occ_grid_node.cpp +++ b/nav2_map_server/test/component/test_occ_grid_node.cpp @@ -16,10 +16,11 @@ #include <rclcpp/rclcpp.hpp> #include <memory> -#include "nav2_map_server/occ_grid_loader.hpp" #include "test_constants/test_constants.h" +#include "nav2_map_server/occ_grid_loader.hpp" +#include "nav2_util/lifecycle_service_client.hpp" -// TODO(bpwilcox): test publisher +using lifecycle_msgs::msg::Transition; class RclCppFixture { @@ -27,6 +28,7 @@ public: RclCppFixture() {rclcpp::init(0, nullptr);} ~RclCppFixture() {rclcpp::shutdown();} }; + RclCppFixture g_rclcppfixture; class TestNode : public ::testing::Test @@ -35,6 +37,17 @@ public: TestNode() { node_ = rclcpp::Node::make_shared("map_client_test"); + lifecycle_client_ = + std::make_shared<nav2_util::LifecycleServiceClient>("map_server", node_); + + lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE); + lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE); + } + + ~TestNode() + { + lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE); + lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP); } template<class T> @@ -45,29 +58,35 @@ public: typename T::Request::SharedPtr request) { auto result = client->async_send_request(request); - // Wait for the result. + + // Wait for the result if (rclcpp::spin_until_future_complete(node, result) == rclcpp::executor::FutureReturnCode::SUCCESS) { return result.get(); } else { - return NULL; + return nullptr; } } protected: rclcpp::Node::SharedPtr node_; + std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_; }; TEST_F(TestNode, ResultReturned) { auto req = std::make_shared<nav_msgs::srv::GetMap::Request>(); auto client = node_->create_client<nav_msgs::srv::GetMap>("map"); + ASSERT_TRUE(client->wait_for_service()); + auto resp = send_request<nav_msgs::srv::GetMap>(node_, client, req); + ASSERT_FLOAT_EQ(resp->map.info.resolution, g_valid_image_res); ASSERT_EQ(resp->map.info.width, g_valid_image_width); ASSERT_EQ(resp->map.info.height, g_valid_image_height); + for (unsigned int i = 0; i < resp->map.info.width * resp->map.info.height; i++) { ASSERT_EQ(g_valid_image_content[i], resp->map.data[i]); } diff --git a/nav2_map_server/test/unit/test_occ_grid.cpp b/nav2_map_server/test/unit/test_occ_grid.cpp index 0291f078dfc72787453fb5f3847a9da6d4e5124e..317cbe129a661f92ba27cb85d1073e57b547fe1a 100644 --- a/nav2_map_server/test/unit/test_occ_grid.cpp +++ b/nav2_map_server/test/unit/test_occ_grid.cpp @@ -35,8 +35,12 @@ #include <string> #include <vector> #include <memory> +#include <iostream> +#include <fstream> +#include "yaml-cpp/yaml.h" #include "nav2_map_server/occ_grid_loader.hpp" +#include "nav2_lifecycle/lifecycle_node.hpp" #include "test_constants/test_constants.h" #define TEST_DIR TEST_DIRECTORY @@ -55,15 +59,43 @@ RclCppFixture g_rclcppfixture; class TestMapLoader : public nav2_map_server::OccGridLoader { + FRIEND_TEST(MapLoaderTest, loadValidPNG); + FRIEND_TEST(MapLoaderTest, loadValidBMP); + FRIEND_TEST(MapLoaderTest, loadInvalidFile); + public: - explicit TestMapLoader(rclcpp::Node * node, YAML::Node & doc) - : OccGridLoader(node, doc) + explicit TestMapLoader(nav2_lifecycle::LifecycleNode::SharedPtr node, std::string yaml_filename) + : OccGridLoader(node, yaml_filename) { } nav_msgs::msg::OccupancyGrid getOccupancyGrid() { - return msg_; + return *msg_; + } +}; + +class FakeMapServer : public nav2_lifecycle::LifecycleNode +{ +public: + FakeMapServer() + : nav2_lifecycle::LifecycleNode("FakeMapServer") {} + + nav2_lifecycle::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override + { + return nav2_lifecycle::CallbackReturn::SUCCESS; + } + nav2_lifecycle::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override + { + return nav2_lifecycle::CallbackReturn::SUCCESS; + } + nav2_lifecycle::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override + { + return nav2_lifecycle::CallbackReturn::SUCCESS; + } + nav2_lifecycle::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override + { + return nav2_lifecycle::CallbackReturn::SUCCESS; } }; @@ -73,35 +105,53 @@ public: MapLoaderTest() { // Set up a fake YAML document with the required fields - doc_["resolution"] = "0.1"; - doc_["origin"][0] = "2.0"; - doc_["origin"][1] = "3.0"; - doc_["origin"][2] = "1.0"; - doc_["negate"] = "0"; - doc_["occupied_thresh"] = "0.65"; - doc_["free_thresh"] = "0.196"; - - node_ = std::make_shared<rclcpp::Node>("map_server"); - map_loader_ = new TestMapLoader(node_.get(), doc_); + + std::string temp_name("/tmp/map_unit_test.yaml"); + std::ofstream params_file; + params_file.open(temp_name, std::ofstream::out | std::ofstream::trunc); + + params_file << "resolution: 0.1" << std::endl; + params_file << "origin: [2.0, 3.0, 1.0]" << std::endl; + params_file << "negate: 0" << std::endl; + params_file << "occupied_thresh: 0.65" << std::endl; + params_file << "free_thresh: 0.196" << std::endl; + + params_file.close(); + + node_ = std::make_shared<FakeMapServer>(); + map_loader_ = std::make_unique<TestMapLoader>(node_, temp_name); } protected: - rclcpp::Node::SharedPtr node_; - TestMapLoader * map_loader_; - YAML::Node doc_; + nav2_lifecycle::LifecycleNode::SharedPtr node_; + std::unique_ptr<TestMapLoader> map_loader_; }; -/* Try to load a valid PNG file. Succeeds if no exception is thrown, and if - * the loaded image matches the known dimensions and content of the file. - * - * This test can fail on OS X, due to an apparent limitation of the - * underlying SDL_Image library. */ +// Try to load a valid PNG file. Succeeds if no exception is thrown, and if +// the loaded image matches the known dimensions and content of the file. +// +// This test can fail on OS X, due to an apparent limitation of the +// underlying SDL_Image library. TEST_F(MapLoaderTest, loadValidPNG) { auto test_png = path(TEST_DIR) / path(g_valid_png_file); - ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_png.string())); + nav2_map_server::OccGridLoader::LoadParameters loadParameters; + loadParameters.resolution = g_valid_image_res; + loadParameters.origin[0] = 0; + loadParameters.origin[1] = 0; + loadParameters.origin[2] = 0; + loadParameters.free_thresh = 0.196; + loadParameters.occupied_thresh = 0.65; + loadParameters.mode = nav2_map_server::OccGridLoader::TRINARY; + loadParameters.negate = 0; + + // In order to loadMapFromFile without going through the Configure and Activate states, + // the msg_ member must be initialized + map_loader_->msg_ = std::make_unique<nav_msgs::msg::OccupancyGrid>(); + + ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_png.string(), &loadParameters)); nav_msgs::msg::OccupancyGrid map_msg = map_loader_->getOccupancyGrid(); EXPECT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res); @@ -112,15 +162,28 @@ TEST_F(MapLoaderTest, loadValidPNG) } } -/* Try to load a valid BMP file. Succeeds if no exception is thrown, and if - * the loaded image matches the known dimensions and content of the file. */ +// Try to load a valid BMP file. Succeeds if no exception is thrown, and if +// the loaded image matches the known dimensions and content of the file. TEST_F(MapLoaderTest, loadValidBMP) { auto test_bmp = path(TEST_DIR) / path(g_valid_bmp_file); - - ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_bmp.string())); + nav2_map_server::OccGridLoader::LoadParameters loadParameters; + loadParameters.resolution = g_valid_image_res; + loadParameters.origin[0] = 0; + loadParameters.origin[1] = 0; + loadParameters.origin[2] = 0; + loadParameters.free_thresh = 0.196; + loadParameters.occupied_thresh = 0.65; + loadParameters.mode = nav2_map_server::OccGridLoader::TRINARY; + loadParameters.negate = 0; + + // In order to loadMapFromFile without going through the Configure and Activate states, + // the msg_ member must be initialized + map_loader_->msg_ = std::make_unique<nav_msgs::msg::OccupancyGrid>(); + + ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_bmp.string(), &loadParameters)); nav_msgs::msg::OccupancyGrid map_msg = map_loader_->getOccupancyGrid(); EXPECT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res); @@ -131,10 +194,22 @@ TEST_F(MapLoaderTest, loadValidBMP) } } -/* Try to load an invalid file. Succeeds if a std::runtime exception is thrown */ +// Try to load an invalid file. Succeeds if a std::runtime exception is thrown TEST_F(MapLoaderTest, loadInvalidFile) { auto test_invalid = path(TEST_DIR) / path("foo"); - ASSERT_THROW(map_loader_->loadMapFromFile(test_invalid.string()), std::runtime_error); + + nav2_map_server::OccGridLoader::LoadParameters loadParameters; + loadParameters.resolution = g_valid_image_res; + loadParameters.origin[0] = 0; + loadParameters.origin[1] = 0; + loadParameters.origin[2] = 0; + loadParameters.free_thresh = 0.196; + loadParameters.occupied_thresh = 0.65; + loadParameters.mode = nav2_map_server::OccGridLoader::TRINARY; + loadParameters.negate = 0; + + ASSERT_THROW(map_loader_->loadMapFromFile( + test_invalid.string(), &loadParameters), std::runtime_error); }