Throw nice errors when creating a publisher with intraprocess communication and incompatible qos policy (#729)
* Throw nice errors when creating a publisher with intraprocess communication and history keep all or history depth 0. Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>
This commit is contained in:
parent
1a662d46fb
commit
30df5cdf36
3 changed files with 60 additions and 25 deletions
|
@ -47,6 +47,14 @@ NodeTopics::create_publisher(
|
|||
// Get the intra process manager instance for this context.
|
||||
auto ipm = context->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
|
||||
// Register the publisher with the intra process manager.
|
||||
if (publisher_options.qos.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) {
|
||||
throw std::invalid_argument(
|
||||
"intraprocess communication is not allowed with keep all history qos policy");
|
||||
}
|
||||
if (publisher_options.qos.depth == 0) {
|
||||
throw std::invalid_argument(
|
||||
"intraprocess communication is not allowed with a zero qos history depth value");
|
||||
}
|
||||
uint64_t intra_process_publisher_id = ipm->add_publisher(publisher);
|
||||
publisher->setup_intra_process(
|
||||
intra_process_publisher_id,
|
||||
|
|
|
@ -265,7 +265,7 @@ PublisherBase::setup_intra_process(
|
|||
{
|
||||
// Intraprocess configuration is not allowed with "durability" qos policy non "volatile".
|
||||
if (this->get_actual_qos().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
|
||||
throw exceptions::InvalidParametersException(
|
||||
throw std::invalid_argument(
|
||||
"intraprocess communication is not allowed with durability qos policy non-volatile");
|
||||
}
|
||||
const char * topic_name = this->get_topic_name();
|
||||
|
|
|
@ -16,6 +16,7 @@
|
|||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
@ -24,12 +25,13 @@
|
|||
|
||||
class TestPublisher : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
public:
|
||||
static void SetUpTestCase()
|
||||
{
|
||||
rclcpp::init(0, nullptr);
|
||||
}
|
||||
|
||||
protected:
|
||||
void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
|
||||
{
|
||||
node = std::make_shared<rclcpp::Node>("my_node", "/ns", node_options);
|
||||
|
@ -43,6 +45,25 @@ protected:
|
|||
rclcpp::Node::SharedPtr node;
|
||||
};
|
||||
|
||||
struct TestParameters
|
||||
{
|
||||
TestParameters(rclcpp::QoS qos, std::string description)
|
||||
: qos(qos), description(description) {}
|
||||
rclcpp::QoS qos;
|
||||
std::string description;
|
||||
};
|
||||
|
||||
std::ostream & operator<<(std::ostream & out, const TestParameters & params)
|
||||
{
|
||||
out << params.description;
|
||||
return out;
|
||||
}
|
||||
|
||||
class TestPublisherInvalidIntraprocessQos
|
||||
: public TestPublisher,
|
||||
public ::testing::WithParamInterface<TestParameters>
|
||||
{};
|
||||
|
||||
class TestPublisherSub : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
|
@ -65,14 +86,6 @@ protected:
|
|||
rclcpp::Node::SharedPtr subnode;
|
||||
};
|
||||
|
||||
static constexpr rmw_qos_profile_t invalid_qos_profile()
|
||||
{
|
||||
rmw_qos_profile_t profile = rmw_qos_profile_default;
|
||||
profile.depth = 1;
|
||||
profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
|
||||
return profile;
|
||||
}
|
||||
|
||||
/*
|
||||
Testing publisher construction and destruction.
|
||||
*/
|
||||
|
@ -165,29 +178,43 @@ TEST_F(TestPublisher, various_creation_signatures) {
|
|||
/*
|
||||
Testing publisher with intraprocess enabled and invalid QoS
|
||||
*/
|
||||
TEST_F(TestPublisher, intraprocess_with_invalid_qos) {
|
||||
TEST_P(TestPublisherInvalidIntraprocessQos, test_publisher_throws) {
|
||||
initialize(rclcpp::NodeOptions().use_intra_process_comms(true));
|
||||
rmw_qos_profile_t qos = invalid_qos_profile();
|
||||
rclcpp::QoS qos = GetParam().qos;
|
||||
using rcl_interfaces::msg::IntraProcessMessage;
|
||||
{
|
||||
#if !defined(_WIN32)
|
||||
# pragma GCC diagnostic push
|
||||
# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
|
||||
#else // !defined(_WIN32)
|
||||
# pragma warning(push)
|
||||
# pragma warning(disable: 4996)
|
||||
#endif
|
||||
ASSERT_THROW(
|
||||
{auto publisher = node->create_publisher<IntraProcessMessage>("topic", qos);},
|
||||
rclcpp::exceptions::InvalidParametersException);
|
||||
#if !defined(_WIN32)
|
||||
# pragma GCC diagnostic pop
|
||||
#else // !defined(_WIN32)
|
||||
# pragma warning(pop)
|
||||
#endif
|
||||
std::invalid_argument);
|
||||
}
|
||||
}
|
||||
|
||||
static std::vector<TestParameters> invalid_qos_profiles()
|
||||
{
|
||||
std::vector<TestParameters> parameters;
|
||||
|
||||
parameters.reserve(3);
|
||||
parameters.push_back(
|
||||
TestParameters(
|
||||
rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(),
|
||||
"transient_local_qos"));
|
||||
parameters.push_back(
|
||||
TestParameters(
|
||||
rclcpp::QoS(rclcpp::KeepLast(0)),
|
||||
"keep_last_qos_with_zero_history_depth"));
|
||||
parameters.push_back(
|
||||
TestParameters(
|
||||
rclcpp::QoS(rclcpp::KeepAll()),
|
||||
"keep_all_qos"));
|
||||
|
||||
return parameters;
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_CASE_P(
|
||||
TestPublisherThrows, TestPublisherInvalidIntraprocessQos,
|
||||
::testing::ValuesIn(invalid_qos_profiles()),
|
||||
::testing::PrintToStringParamName());
|
||||
|
||||
/*
|
||||
Testing publisher construction and destruction for subnodes.
|
||||
*/
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue