Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add map_topic to static layer and amcl #1910

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
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 @@ -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 |

---

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 @@ -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);
Expand Down Expand Up @@ -1277,7 +1282,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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not make them all private and remove the global one? I don't have a particular issue with this but curious the justification for it.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I thought it would be good to keep it compatible with the global parameter so the user can choose which one to use depending on whatever is the most convenient. In the future if any layers are added that also subscribe to map topics like the static layer then the global parameter may be advantageous whereas the private parameter comes in handy if you need to have subscriptions to an ancillary map topics.

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