Skip to content

Commit

Permalink
Solve bug when CostmapInfoServer is reactivated (ros-navigation#3292)
Browse files Browse the repository at this point in the history
* Solve bug when CostmapInfoServer is reactivated
  • Loading branch information
MartiBolet authored and Joshua Wallace committed Nov 30, 2022
1 parent ebf079e commit ba3d647
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 9 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ class CostmapFilterInfoServer : public nav2_util::LifecycleNode
private:
rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr publisher_;

std::unique_ptr<nav2_msgs::msg::CostmapFilterInfo> msg_;
nav2_msgs::msg::CostmapFilterInfo msg_;
}; // CostmapFilterInfoServer

} // namespace nav2_map_server
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -48,13 +48,13 @@ CostmapFilterInfoServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
publisher_ = this->create_publisher<nav2_msgs::msg::CostmapFilterInfo>(
filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());

msg_ = std::make_unique<nav2_msgs::msg::CostmapFilterInfo>();
msg_->header.frame_id = "";
msg_->header.stamp = now();
msg_->type = get_parameter("type").as_int();
msg_->filter_mask_topic = get_parameter("mask_topic").as_string();
msg_->base = static_cast<float>(get_parameter("base").as_double());
msg_->multiplier = static_cast<float>(get_parameter("multiplier").as_double());
msg_ = nav2_msgs::msg::CostmapFilterInfo();
msg_.header.frame_id = "";
msg_.header.stamp = now();
msg_.type = get_parameter("type").as_int();
msg_.filter_mask_topic = get_parameter("mask_topic").as_string();
msg_.base = static_cast<float>(get_parameter("base").as_double());
msg_.multiplier = static_cast<float>(get_parameter("multiplier").as_double());

return nav2_util::CallbackReturn::SUCCESS;
}
Expand All @@ -65,7 +65,8 @@ CostmapFilterInfoServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
RCLCPP_INFO(get_logger(), "Activating");

publisher_->on_activate();
publisher_->publish(std::move(msg_));
auto costmap_filter_info = std::make_unique<nav2_msgs::msg::CostmapFilterInfo>(msg_);
publisher_->publish(std::move(costmap_filter_info));

// create bond connection
createBond();
Expand Down
31 changes: 31 additions & 0 deletions nav2_map_server/test/unit/test_costmap_filter_info_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,16 @@ class InfoServerWrapper : public nav2_map_server::CostmapFilterInfoServer
on_cleanup(get_current_state());
on_shutdown(get_current_state());
}

void deactivate()
{
on_deactivate(get_current_state());
}

void activate()
{
on_activate(get_current_state());
}
};

class InfoServerTester : public ::testing::Test
Expand Down Expand Up @@ -144,3 +154,24 @@ TEST_F(InfoServerTester, testCostmapFilterInfoPublish)
EXPECT_NEAR(info_->base, BASE, EPSILON);
EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON);
}

TEST_F(InfoServerTester, testCostmapFilterInfoDeactivateActivate)
{
info_server_->deactivate();
info_ = nullptr;
info_server_->activate();

rclcpp::Time start_time = info_server_->now();
while (!isReceived()) {
rclcpp::spin_some(info_server_->get_node_base_interface());
std::this_thread::sleep_for(100ms);
// Waiting no more than 5 seconds
ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms));
}

// Checking received CostmapFilterInfo for consistency
EXPECT_EQ(info_->type, TYPE);
EXPECT_EQ(info_->filter_mask_topic, MASK_TOPIC);
EXPECT_NEAR(info_->base, BASE, EPSILON);
EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON);
}

0 comments on commit ba3d647

Please sign in to comment.