Skip to content

Commit

Permalink
Added map_topic parameters to static layer and amcl (#1910)
Browse files Browse the repository at this point in the history
  • Loading branch information
Michael-Equi authored and SteveMacenski committed Aug 11, 2020
1 parent 27abf03 commit e4c2ab1
Show file tree
Hide file tree
Showing 4 changed files with 18 additions and 2 deletions.
2 changes: 2 additions & 0 deletions doc/parameters/param_list.md
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
| `<static layer>`.subscribe_to_updates | false | Subscribe to static map updates after receiving first |
| `<static layer>`.map_subscribe_transient_local | true | QoS settings for map topic |
| `<static layer>`.transform_tolerance | 0.0 | TF tolerance |
| `<static layer>`.map_topic | "" | Name of the map topic to subscribe to (empty means use the map_topic defined by `costmap_2d_ros`) |

## inflation_layer plugin

Expand Down Expand Up @@ -560,6 +561,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 |

---

Expand Down
1 change: 1 addition & 0 deletions nav2_amcl/include/nav2_amcl/amcl_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
7 changes: 6 additions & 1 deletion nav2_amcl/src/amcl_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -1096,6 +1100,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);
Expand Down Expand Up @@ -1271,7 +1276,7 @@ AmclNode::initPubSub()
std::bind(&AmclNode::initialPoseReceived, this, std::placeholders::_1));

map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
"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.");
Expand Down
10 changes: 9 additions & 1 deletion nav2_costmap_2d/plugins/static_layer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand Down

0 comments on commit e4c2ab1

Please sign in to comment.