diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 72ddc23be2..a83f1f3334 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -283,6 +283,44 @@ class SubscriptionBase : public std::enable_shared_from_this std::vector get_network_flow_endpoints() const; + /// Check if content filtered topic feature of the subscription instance is enabled. + /** + * \return boolean flag indicating if the content filtered topic of this subscription is enabled. + */ + RCLCPP_PUBLIC + bool + is_cft_enabled() const; + + /// Set the filter expression and expression parameters for the subscription. + /** + * \param[in] filter_expression An filter expression to set. + * \sa SubscriptionOptionsBase::ContentFilterOptions::filter_expression + * \param[in] expression_parameters Array of expression parameters to set. + * \sa SubscriptionOptionsBase::ContentFilterOptions::expression_parameters + * \throws RCLBadAlloc if memory cannot be allocated + * \throws RCLError if an unexpect error occurs + */ + RCLCPP_PUBLIC + virtual + void + set_content_filter( + const std::string & filter_expression, + const std::vector & expression_parameters = {}); + + /// Get the filter expression and expression parameters for the subscription. + /** + * \param[out] filter_expression An filter expression to get. + * \param[out] expression_parameters Array of expression parameters to get. + * \throws RCLBadAlloc if memory cannot be allocated + * \throws RCLError if an unexpect error occurs + */ + RCLCPP_PUBLIC + virtual + void + get_content_filter( + std::string & filter_expression, + std::vector & expression_parameters) const; + protected: template void diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index d6d2d4c60d..e44704c7b0 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -81,6 +81,20 @@ struct SubscriptionOptionsBase TopicStatisticsOptions topic_stats_options; QosOverridingOptions qos_overriding_options; + + /// Options to configure content filtered topic in the subscription. + struct ContentFilterOptions + { + /// Filter expression is similar to the WHERE part of an SQL clause + std::string filter_expression; + /** + * Expression parameters is the tokens placeholder ‘parameters’ (i.e., "%n" tokens begin from 0) + * in the filter_expression + */ + std::vector expression_parameters; + }; + + ContentFilterOptions content_filter_options; }; /// Structure containing optional configuration for Subscriptions. @@ -123,6 +137,21 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options); } + // Copy content_filter_options into rcl_subscription_options. + if (!content_filter_options.filter_expression.empty()) { + std::vector cstrings = + get_c_vector_string(content_filter_options.expression_parameters); + rcl_ret_t ret = rcl_subscription_options_set_content_filter_options( + get_c_string(content_filter_options.filter_expression), + cstrings.size(), + cstrings.data(), + &result); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to set content_filter_options"); + } + } + return result; } diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 5011a12c6a..0a659f9a26 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -289,6 +289,15 @@ RCLCPP_PUBLIC const char * get_c_string(const std::string & string_in); +/// Return the std::vector of C string from the given std::vector. +/** + * \param[in] string_in is a std::string + * \return the std::vector of C string from the std::vector + */ +RCLCPP_PUBLIC +std::vector +get_c_vector_string(const std::vector & strings); + } // namespace rclcpp #endif // RCLCPP__UTILITIES_HPP_ diff --git a/rclcpp/include/rclcpp/wait_for_message.hpp b/rclcpp/include/rclcpp/wait_for_message.hpp index 20c9df4db6..352fae022a 100644 --- a/rclcpp/include/rclcpp/wait_for_message.hpp +++ b/rclcpp/include/rclcpp/wait_for_message.hpp @@ -56,6 +56,9 @@ bool wait_for_message( wait_set.add_subscription(subscription); wait_set.add_guard_condition(gc); auto ret = wait_set.wait(time_to_wait); + // if it is fixed in other PR, remove them and rebase. + wait_set.remove_subscription(subscription); + wait_set.remove_guard_condition(gc); if (ret.kind() != rclcpp::WaitResultKind::Ready) { return false; } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 472034f362..29554fcd43 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -19,6 +19,8 @@ #include #include +#include "rcpputils/scope_exit.hpp" + #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" @@ -326,3 +328,92 @@ std::vector SubscriptionBase::get_network_flow_endp return network_flow_endpoint_vector; } + +bool +SubscriptionBase::is_cft_enabled() const +{ + return rcl_subscription_is_cft_enabled(subscription_handle_.get()); +} + +void +SubscriptionBase::set_content_filter( + const std::string & filter_expression, + const std::vector & expression_parameters) +{ + rcl_subscription_content_filter_options_t options = + rcl_subscription_get_default_content_filter_options(); + + std::vector cstrings = + get_c_vector_string(expression_parameters); + rcl_ret_t ret = rcl_subscription_content_filter_options_init( + get_c_string(filter_expression), + cstrings.size(), + cstrings.data(), + &options); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to init subscription content_filtered_topic option"); + } + RCPPUTILS_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Failed to fini subscription content_filtered_topic option: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }); + + ret = rcl_subscription_set_content_filter( + subscription_handle_.get(), + &options); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set cft expression parameters"); + } +} + +void +SubscriptionBase::get_content_filter( + std::string & filter_expression, + std::vector & expression_parameters) const +{ + rcl_subscription_content_filter_options_t options = + rcl_subscription_get_default_content_filter_options(); + + // use rcl_content_filter_options_t instead of char * and + rcl_ret_t ret = rcl_subscription_get_content_filter( + subscription_handle_.get(), + &options); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get cft expression parameters"); + } + + RCPPUTILS_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Failed to fini subscription content_filtered_topic option: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }); + + rmw_subscription_content_filter_options_t * content_filter_options = + options.rmw_subscription_content_filter_options; + if (content_filter_options->filter_expression) { + filter_expression = content_filter_options->filter_expression; + } + + if (content_filter_options->expression_parameters) { + for (size_t i = 0; i < content_filter_options->expression_parameters->size; ++i) { + expression_parameters.push_back( + content_filter_options->expression_parameters->data[i]); + } + } +} diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 140b5531d2..1e19c96e5b 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -126,10 +126,19 @@ void TimeSource::attachNode( return result; }); + const rclcpp::QoS qos = ( + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)) + ); + rclcpp::SubscriptionOptions options; + options.content_filter_options.filter_expression = + std::string("node = '") + node_base_->get_fully_qualified_name() + "'"; + // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( node_topics_, - std::bind(&TimeSource::on_parameter_event, this, std::placeholders::_1)); + std::bind(&TimeSource::on_parameter_event, this, std::placeholders::_1), + qos, + options); } void TimeSource::detachNode() @@ -299,6 +308,11 @@ void TimeSource::on_parameter_event( { // Filter out events on 'use_sim_time' parameter instances in other nodes. if (event->node != node_base_->get_fully_qualified_name()) { + if (parameter_subscription_->is_cft_enabled()) { + RCLCPP_ERROR( + logger_, "this node '%s' should not get parameter event of another node '%s'", + node_base_->get_fully_qualified_name(), event->node.c_str()); + } return; } // Filter for only 'use_sim_time' being added or changed. diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index c3e3915d79..55a45d62d2 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -203,4 +203,17 @@ get_c_string(const std::string & string_in) return string_in.c_str(); } +std::vector +get_c_vector_string(const std::vector & strings) +{ + std::vector cstrings; + cstrings.reserve(strings.size()); + + for (size_t i = 0; i < strings.size(); ++i) { + cstrings.push_back(strings[i].c_str()); + } + + return cstrings; +} + } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index d78a1adef5..619867f8b2 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -739,3 +739,20 @@ ament_add_gtest(test_graph_listener test_graph_listener.cpp) if(TARGET test_graph_listener) target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) endif() + +function(test_subscription_content_filter_for_rmw_implementation) + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + ament_add_gmock(test_subscription_content_filter${target_suffix} + test_subscription_content_filter.cpp + ENV ${rmw_implementation_env_var} + ) + if(TARGET test_subscription_content_filter${target_suffix}) + target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick) + ament_target_dependencies(test_subscription_content_filter${target_suffix} + "rcpputils" + "rosidl_typesupport_cpp" + "test_msgs" + ) + endif() +endfunction() +call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation) diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp new file mode 100644 index 0000000000..ab6998517c --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -0,0 +1,309 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_for_message.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/basic_types.hpp" +#include "test_msgs/msg/empty.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestContentFilterSubscription, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_content_filter_node", "/ns"); + context = node->get_node_options().context(); + qos.reliable().transient_local(); + + auto options = rclcpp::SubscriptionOptions(); + options.content_filter_options.filter_expression = filter_expression_init; + options.content_filter_options.expression_parameters = expression_parameters_1; + + auto callback = [](std::shared_ptr) {}; + sub = node->create_subscription( + "content_filter_topic", qos, callback, options); + } + + void TearDown() + { + sub.reset(); + node.reset(); + } + + template + bool wait_for(const Condition & condition, const Duration & timeout) + { + using clock = std::chrono::system_clock; + auto start = clock::now(); + while (!condition()) { + if ((clock::now() - start) > timeout) { + return false; + } + rclcpp::spin_some(node); + } + return true; + } + +protected: + rclcpp::Node::SharedPtr node; + rclcpp::Context::SharedPtr context; + rclcpp::QoS qos{rclcpp::KeepLast{10}}; + rclcpp::Subscription::SharedPtr sub; + + std::string filter_expression_init = "int32_value = %0"; + std::vector expression_parameters_1 = {"3"}; + std::vector expression_parameters_2 = {"4"}; +}; + +bool operator==(const test_msgs::msg::BasicTypes & m1, const test_msgs::msg::BasicTypes & m2) +{ + return m1.bool_value == m2.bool_value && + m1.byte_value == m2.byte_value && + m1.char_value == m2.char_value && + m1.float32_value == m2.float32_value && + m1.float64_value == m2.float64_value && + m1.int8_value == m2.int8_value && + m1.uint8_value == m2.uint8_value && + m1.int16_value == m2.int16_value && + m1.uint16_value == m2.uint16_value && + m1.int32_value == m2.int32_value && + m1.uint32_value == m2.uint32_value && + m1.int64_value == m2.int64_value && + m1.uint64_value == m2.uint64_value; +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), is_cft_enabled) { + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_is_cft_enabled, false); + EXPECT_FALSE(sub->is_cft_enabled()); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_is_cft_enabled, true); + EXPECT_TRUE(sub->is_cft_enabled()); + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_content_filter, RCL_RET_ERROR); + + std::string filter_expression; + std::vector expression_parameters; + EXPECT_THROW( + sub->get_content_filter(filter_expression, expression_parameters), + rclcpp::exceptions::RCLError); +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_set_content_filter, RCL_RET_ERROR); + + std::string filter_expression = "int32_value = %0"; + std::string expression_parameter = "100"; + EXPECT_THROW( + sub->set_content_filter(filter_expression, {expression_parameter}), + rclcpp::exceptions::RCLError); +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter) { + std::string filter_expression; + std::vector expression_parameters; + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->get_content_filter(filter_expression, expression_parameters)); + + EXPECT_EQ(filter_expression, filter_expression_init); + EXPECT_EQ(expression_parameters, expression_parameters_1); + } else { + EXPECT_THROW( + sub->get_content_filter(filter_expression, expression_parameters), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter) { + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + } else { + EXPECT_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_begin) { + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 5s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 3; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 3; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + EXPECT_FALSE(receive); + } + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_later) { + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 5s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + if (sub->is_cft_enabled()) { + EXPECT_FALSE(receive); + } else { + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_reset) { + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 5s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + if (sub->is_cft_enabled()) { + EXPECT_FALSE(receive); + } else { + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter("")); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + } +} diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 8294f2be68..7887a5338e 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -158,6 +158,18 @@ class ClientBase : public rclcpp::Waitable GoalUUID generate_goal_id(); + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + bool + add_goal_uuid(const GoalUUID & goal_uuid); + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + bool + remove_goal_uuid(const GoalUUID & goal_uuid); + /// \internal RCLCPP_ACTION_PUBLIC virtual @@ -715,6 +727,15 @@ class Client : public ClientBase continue; } goal_handle->set_status(status.status); + switch (status.status) { + case GoalStatus::STATUS_SUCCEEDED: + case GoalStatus::STATUS_ABORTED: + case GoalStatus::STATUS_CANCELED: + static_cast(remove_goal_uuid(goal_id)); + break; + default: + break; + } } } diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 5839a4f932..b1482d4900 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -120,6 +120,8 @@ class ClientBaseImpl std::independent_bits_engine< std::default_random_engine, 8, unsigned int> random_bytes_generator; + + std::mutex goal_uuids_mutex; }; ClientBase::ClientBase( @@ -302,9 +304,26 @@ ClientBase::send_goal_request(std::shared_ptr request, ResponseCallback ca { std::unique_lock guard(pimpl_->goal_requests_mutex); int64_t sequence_number; + // goal_id, which type is unique_identifier_msgs::msg::UUID, + // is the first member in ActionT::Impl::SendGoalService::Request + auto goal_id = std::static_pointer_cast(request); + if (!add_goal_uuid(goal_id->uuid)) { + RCLCPP_DEBUG( + get_logger(), + "failed to add goal uuid for setting content filtered topic for action subscriptions: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } rcl_ret_t ret = rcl_action_send_goal_request( pimpl_->client_handle.get(), request.get(), &sequence_number); if (RCL_RET_OK != ret) { + if (!remove_goal_uuid(goal_id->uuid)) { + RCLCPP_DEBUG( + get_logger(), + "failed to remove goal uuid: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); } assert(pimpl_->pending_goal_responses.count(sequence_number) == 0); @@ -383,6 +402,33 @@ ClientBase::generate_goal_id() return goal_id; } +bool +ClientBase::add_goal_uuid(const GoalUUID & goal_uuid) +{ + std::lock_guard guard(pimpl_->goal_uuids_mutex); + + rcl_ret_t ret = rcl_action_add_goal_uuid( + pimpl_->client_handle.get(), + static_cast(goal_uuid.data())); + if (RCL_RET_OK != ret) { + return false; + } + return true; +} + +bool +ClientBase::remove_goal_uuid(const GoalUUID & goal_uuid) +{ + std::lock_guard guard(pimpl_->goal_uuids_mutex); + rcl_ret_t ret = rcl_action_remove_goal_uuid( + pimpl_->client_handle.get(), + static_cast(goal_uuid.data())); + if (RCL_RET_OK != ret) { + return false; + } + return true; +} + std::shared_ptr ClientBase::take_data() {