Bug report
Required Info:
- Operating System:
- Computer:
- 13th Gen Intel(R) Core(TM) i5-13450HX
- ROS2 Version:
- Version or commit hash:
- ros-jazzy-navigation2 1.3.12-1noble.20260615.181551
- DDS implementation:
Steps to reproduce issue
Launch a navigation stack, which has at least a map_server and a planner_server. In you configuration file, set the 'always_send_full_costmap' to false at global_costmap. Here is my relevant config:
planner_server:
ros__parameters:
expected_planner_frequency: 1.0
planner_plugins: ["GridBased"]
costmap_update_timeout: 1.0
GridBased:
plugin: "nav2_smac_planner::SmacPlannerLattice"
downsample_costmap: false
downsampling_factor: 1
lookup_table_size: 20.0
allow_unknown: false
tolerance: 0.25
max_iterations: 1000000
max_on_approach_iterations: 1000
max_planning_time: 5.0
analytic_expansion_ratio: 3.5
analytic_expansion_max_length: 6.0
analytic_expansion_max_cost: 200.0
analytic_expansion_max_cost_override: false
reverse_penalty: 3.1
change_penalty: 0.15
non_straight_penalty: 1.15
cost_penalty: 2.0
rotation_penalty: 5.0
retrospective_penalty: 0.15
lattice_filepath: $(find-pkg-share research_project)/config/motion_primitives/acker_minturn_05_costres_005.json
cache_obstacle_heuristic: true
allow_reverse_expansion: true
smooth_path: true
smoother:
max_iterations: 1000
w_smooth: 0.15
w_data: 0.2
tolerance: 0.0000000001
do_refinement: false
refinement_num: 0
terminal_checking_interval: 5000
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
footprint_padding: 0.05
robot_radius: 0.22
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "inflation_layer"]
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: true
enabled: true
subscribe_to_updates: true
transform_tolerance: 0.1
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
enabled: true
cost_scaling_factor: 1.5
inflation_radius: 3.9
inflate_unknown: false
inflate_around_unknown: true
always_send_full_costmap: false
map_server:
ros__parameters:
topic_name: "/map"
frame_id: "map"
Expected behavior
After launching your Nav2 nodes, when you subscriber to the topic '/global_costmap/costmap', you receive the costmap.
Actual behavior
After launching your Nav2 nodes, when you subscriber to the topic '/global_costmap/costmap', you don't receive the costmap. The costmap has never been published, and you cannot trigger the publication, so you won't be able to get the costmap on the topic.
But if you have a subscriber on the topic '/global_costmap/costmap' before launching the nodes, you will receive the costmap. In this case late joining subscribers also receive it as expected.
Reproduction instructions
After launching your Nav2 nodes, run:
ros2 topic echo /global_costmap/costmap
You won't receive any messages.
Now don't kill the created subscriber, but kill the Nav2 nodes. Then relaunch them. Now you will receive the costmap, and if you run the command again inside another terminal, you will also receive it.
Additional information
I did some research, and I have found the reason of this behavior in Jazzy.
There weren't any problems with my setup, the QoS settings match.
There aren't problems with the DDS or with rclcpp. If I create a random publisher with transient local QoS durability and publish a message, and later subscribe to this topic, I receive the message as expected.
The global_costmap receives the map on the /map topic from the map_server, so it is also not the problem. I can obtain it with global_costmap's proper service. Only on the topic it cannot be received.
If you try to reproduce this in older distributions, (I have tried it in Humble,) you will receive the costmap on the topic as expected.
I have found what has changed from Iron to Jazzy, which results this. The costmap is never published on the topic in Jazzy, if there aren't any subscribers on the topic before launch (or more specifically when the map is received on /map topic, but it happens during the activation phase).
In Iron and older versions, in costmap_2d_publisher's publishCostmap() function, updating the grid parameters is done (by prepareGrid() function) if there are subscribers, and the costmap gets published when this happens. After that it is impossible reach the costmap pulishing part, because the grid has been set. But it is not a problem, because it has been published once, and the QoS durability is transient local. Here is the Iron version of the mentioned functions:
void Costmap2DPublisher::publishCostmap()
{
if (costmap_raw_pub_->get_subscription_count() > 0) {
prepareCostmap();
costmap_raw_pub_->publish(std::move(costmap_raw_));
}
float resolution = costmap_->getResolution();
if (always_send_full_costmap_ || grid_resolution != resolution ||
grid_width != costmap_->getSizeInCellsX() ||
grid_height != costmap_->getSizeInCellsY() ||
saved_origin_x_ != costmap_->getOriginX() ||
saved_origin_y_ != costmap_->getOriginY())
{
if (costmap_pub_->get_subscription_count() > 0) {
prepareGrid();
costmap_pub_->publish(std::move(grid_));
}
} else if (x0_ < xn_) {
if (costmap_update_pub_->get_subscription_count() > 0) {
std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
// Publish Just an Update
auto update = std::make_unique<map_msgs::msg::OccupancyGridUpdate>();
update->header.stamp = rclcpp::Time();
update->header.frame_id = global_frame_;
update->x = x0_;
update->y = y0_;
update->width = xn_ - x0_;
update->height = yn_ - y0_;
update->data.resize(update->width * update->height);
unsigned int i = 0;
for (unsigned int y = y0_; y < yn_; y++) {
for (unsigned int x = x0_; x < xn_; x++) {
unsigned char cost = costmap_->getCost(x, y);
update->data[i++] = cost_translation_table_[cost];
}
}
costmap_update_pub_->publish(std::move(update));
}
}
xn_ = yn_ = 0;
x0_ = costmap_->getSizeInCellsX();
y0_ = costmap_->getSizeInCellsY();
}
void Costmap2DPublisher::prepareGrid()
{
std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
grid_resolution = costmap_->getResolution();
grid_width = costmap_->getSizeInCellsX();
grid_height = costmap_->getSizeInCellsY();
grid_ = std::make_unique<nav_msgs::msg::OccupancyGrid>();
grid_->header.frame_id = global_frame_;
grid_->header.stamp = clock_->now();
grid_->info.resolution = grid_resolution;
grid_->info.width = grid_width;
grid_->info.height = grid_height;
double wx, wy;
costmap_->mapToWorld(0, 0, wx, wy);
grid_->info.origin.position.x = wx - grid_resolution / 2;
grid_->info.origin.position.y = wy - grid_resolution / 2;
grid_->info.origin.position.z = 0.0;
grid_->info.origin.orientation.w = 1.0;
saved_origin_x_ = costmap_->getOriginX();
saved_origin_y_ = costmap_->getOriginY();
grid_->data.resize(grid_->info.width * grid_->info.height);
unsigned char * data = costmap_->getCharMap();
for (unsigned int i = 0; i < grid_->data.size(); i++) {
grid_->data[i] = cost_translation_table_[data[i]];
}
}
In Jazzy in contrast, updating the grid parameters does not require subscribers on the topic, it happens when it first receives the map from map_server. It is done by the updateGridParams() function. But if there aren't subscribers, the costmap isn't published that time, and later it is impossible to reach the costmap publishing part, because the grid has been set. This is a problem, because it means that the costmap has never been published. Here you can see the Jazzy version of the mentioned functions:
void Costmap2DPublisher::publishCostmap()
{
auto const costmap_layer = dynamic_cast<CostmapLayer *>(costmap_);
if (costmap_layer != nullptr && !costmap_layer->isEnabled()) {
return;
}
float resolution = costmap_->getResolution();
if (always_send_full_costmap_ || grid_resolution_ != resolution ||
grid_width_ != costmap_->getSizeInCellsX() ||
grid_height_ != costmap_->getSizeInCellsY() ||
saved_origin_x_ != costmap_->getOriginX() ||
saved_origin_y_ != costmap_->getOriginY())
{
updateGridParams();
if (costmap_pub_->get_subscription_count() > 0) {
prepareGrid();
costmap_pub_->publish(std::move(grid_));
}
if (costmap_raw_pub_->get_subscription_count() > 0) {
prepareCostmap();
costmap_raw_pub_->publish(std::move(costmap_raw_));
}
} else if (x0_ < xn_) {
// Publish just update msgs
std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
if (costmap_update_pub_->get_subscription_count() > 0) {
costmap_update_pub_->publish(createGridUpdateMsg());
}
if (costmap_raw_update_pub_->get_subscription_count() > 0) {
costmap_raw_update_pub_->publish(createCostmapUpdateMsg());
}
}
xn_ = yn_ = 0;
x0_ = costmap_->getSizeInCellsX();
y0_ = costmap_->getSizeInCellsY();
}
void Costmap2DPublisher::updateGridParams()
{
saved_origin_x_ = costmap_->getOriginX();
saved_origin_y_ = costmap_->getOriginY();
grid_resolution_ = costmap_->getResolution();
grid_width_ = costmap_->getSizeInCellsX();
grid_height_ = costmap_->getSizeInCellsY();
}
If you place updateGridParams() inside "if (costmap_pub_->get_subscription_count() > 0) {...}", the problem disappears.
Bug report
Required Info:
Steps to reproduce issue
Launch a navigation stack, which has at least a map_server and a planner_server. In you configuration file, set the 'always_send_full_costmap' to false at global_costmap. Here is my relevant config:
Expected behavior
After launching your Nav2 nodes, when you subscriber to the topic '/global_costmap/costmap', you receive the costmap.
Actual behavior
After launching your Nav2 nodes, when you subscriber to the topic '/global_costmap/costmap', you don't receive the costmap. The costmap has never been published, and you cannot trigger the publication, so you won't be able to get the costmap on the topic.
But if you have a subscriber on the topic '/global_costmap/costmap' before launching the nodes, you will receive the costmap. In this case late joining subscribers also receive it as expected.
Reproduction instructions
After launching your Nav2 nodes, run:
You won't receive any messages.
Now don't kill the created subscriber, but kill the Nav2 nodes. Then relaunch them. Now you will receive the costmap, and if you run the command again inside another terminal, you will also receive it.
Additional information
I did some research, and I have found the reason of this behavior in Jazzy.
There weren't any problems with my setup, the QoS settings match.
There aren't problems with the DDS or with rclcpp. If I create a random publisher with transient local QoS durability and publish a message, and later subscribe to this topic, I receive the message as expected.
The global_costmap receives the map on the /map topic from the map_server, so it is also not the problem. I can obtain it with global_costmap's proper service. Only on the topic it cannot be received.
If you try to reproduce this in older distributions, (I have tried it in Humble,) you will receive the costmap on the topic as expected.
I have found what has changed from Iron to Jazzy, which results this. The costmap is never published on the topic in Jazzy, if there aren't any subscribers on the topic before launch (or more specifically when the map is received on /map topic, but it happens during the activation phase).
In Iron and older versions, in costmap_2d_publisher's publishCostmap() function, updating the grid parameters is done (by prepareGrid() function) if there are subscribers, and the costmap gets published when this happens. After that it is impossible reach the costmap pulishing part, because the grid has been set. But it is not a problem, because it has been published once, and the QoS durability is transient local. Here is the Iron version of the mentioned functions:
In Jazzy in contrast, updating the grid parameters does not require subscribers on the topic, it happens when it first receives the map from map_server. It is done by the updateGridParams() function. But if there aren't subscribers, the costmap isn't published that time, and later it is impossible to reach the costmap publishing part, because the grid has been set. This is a problem, because it means that the costmap has never been published. Here you can see the Jazzy version of the mentioned functions:
If you place updateGridParams() inside "if (costmap_pub_->get_subscription_count() > 0) {...}", the problem disappears.