From 07601601917eeeab94782562610fc7c4d66afa6a Mon Sep 17 00:00:00 2001 From: Michael Equi Date: Tue, 4 Aug 2020 14:12:29 -0700 Subject: [PATCH] Added map_topic parameters to static layer and amcl --- doc/parameters/param_list.md | 2 ++ nav2_amcl/include/nav2_amcl/amcl_node.hpp | 1 + nav2_amcl/src/amcl_node.cpp | 7 ++++++- nav2_costmap_2d/plugins/static_layer.cpp | 10 +++++++++- 4 files changed, 18 insertions(+), 2 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 3bbacfa0902..1684789885f 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -78,6 +78,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo | ``.subscribe_to_updates | false | Subscribe to static map updates after receiving first | | ``.map_subscribe_transient_local | true | QoS settings for map topic | | ``.transform_tolerance | 0.0 | TF tolerance | +| ``.map_topic | "" | Name of the map topic to subscribe to (empty means use the map_topic defined by `costmap_2d_ros`) | ## inflation_layer plugin @@ -561,6 +562,7 @@ When `recovery_plugins` parameter is not overridden, the following default plugi | z_short | 0.05 | Mixture weight for z_short part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. | | always_reset_initial_pose | false | Requires that AMCL is provided an initial pose either via topic or initial_pose* parameter (with parameter set_initial_pose: true) when reset. Otherwise, by default AMCL will use the last known pose to initialize | | scan_topic | scan | Topic to subscribe to in order to receive the laser scan for localization | +| map_topic | map | Topic to subscribe to in order to receive the map for localization | --- diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 562af9f128b..dc86baaf6e1 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -249,6 +249,7 @@ class AmclNode : public nav2_util::LifecycleNode double z_short_; double z_rand_; std::string scan_topic_{"scan"}; + std::string map_topic_{"map"}; }; } // namespace nav2_amcl diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index d31adf34718..86571e81f8e 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -217,6 +217,10 @@ AmclNode::AmclNode() add_parameter( "scan_topic", rclcpp::ParameterValue("scan"), "Topic to subscribe to in order to receive the laser scan for localization"); + + add_parameter( + "map_topic", rclcpp::ParameterValue("map"), + "Topic to subscribe to in order to receive the map to localize on"); } AmclNode::~AmclNode() @@ -1102,6 +1106,7 @@ AmclNode::initParameters() get_parameter("first_map_only_", first_map_only_); get_parameter("always_reset_initial_pose", always_reset_initial_pose_); get_parameter("scan_topic", scan_topic_); + get_parameter("map_topic", map_topic_); save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); transform_tolerance_ = tf2::durationFromSec(tmp_tol); @@ -1277,7 +1282,7 @@ AmclNode::initPubSub() std::bind(&AmclNode::initialPoseReceived, this, std::placeholders::_1)); map_sub_ = create_subscription( - "map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + map_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&AmclNode::mapReceived, this, std::placeholders::_1)); RCLCPP_INFO(get_logger(), "Subscribed to map topic."); diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 411981813f6..cb3a01f1be4 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -124,10 +124,18 @@ StaticLayer::getParameters() declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false)); declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); + declareParameter("map_topic", rclcpp::ParameterValue("")); node_->get_parameter(name_ + "." + "enabled", enabled_); node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); - node_->get_parameter("map_topic", map_topic_); + std::string private_map_topic, global_map_topic; + node_->get_parameter(name_ + "." + "map_topic", private_map_topic); + node_->get_parameter("map_topic", global_map_topic); + if (!private_map_topic.empty()) { + map_topic_ = private_map_topic; + } else { + map_topic_ = global_map_topic; + } node_->get_parameter( name_ + "." + "map_subscribe_transient_local", map_subscribe_transient_local_);