diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 54235f0..9344a6e 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -56,6 +56,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/node_interfaces/node_topics.cpp src/rclcpp/parameter.cpp src/rclcpp/parameter_client.cpp + src/rclcpp/parameter_events_filter.cpp src/rclcpp/parameter_service.cpp src/rclcpp/publisher.cpp src/rclcpp/service.cpp @@ -185,6 +186,16 @@ if(BUILD_TESTING) ) target_link_libraries(test_node ${PROJECT_NAME}) endif() + ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp) + if(TARGET test_parameter_events_filter) + target_include_directories(test_parameter_events_filter PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ${rosidl_generator_cpp_INCLUDE_DIRS} + ${rosidl_typesupport_cpp_INCLUDE_DIRS} + ) + target_link_libraries(test_parameter_events_filter ${PROJECT_NAME}) + endif() ament_add_gtest(test_publisher test/test_publisher.cpp) if(TARGET test_publisher) target_include_directories(test_publisher PUBLIC diff --git a/rclcpp/include/rclcpp/parameter_events_filter.hpp b/rclcpp/include/rclcpp/parameter_events_filter.hpp new file mode 100644 index 0000000..fa5b1de --- /dev/null +++ b/rclcpp/include/rclcpp/parameter_events_filter.hpp @@ -0,0 +1,78 @@ +// Copyright 2017 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. + +#ifndef RCLCPP__PARAMETER_EVENTS_FILTER_HPP_ +#define RCLCPP__PARAMETER_EVENTS_FILTER_HPP_ + +#include +#include +#include +#include + +#include "rcl_interfaces/msg/parameter.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "rcl_interfaces/msg/parameter_value.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" + +namespace rclcpp +{ + +class ParameterEventsFilter +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsFilter) + enum class EventType {NEW, DELETED, CHANGED}; ///< An enum for the type of event. + /// Used for the listed results + using EventPair = std::pair; + + /// Construct a filtered view of a parameter event. + /** + * \param[in] event The parameter event message to filter. + * \param[in] names A list of parameter names of interest. + * \param[in] types A list of the types of parameter events of iterest. + * EventType NEW, DELETED, or CHANGED + * + * Example Usage: + * If you have recieved a parameter event and are only interested in parameters foo and + * bar being added or changed but don't care about deletion. + * auto res = rclcpp::ParameterEventsFilter( + * event_shared_ptr, + * {"foo", "bar"}, + * {rclcpp::ParameterEventsFilter::EventType::NEW, rclcpp::ParameterEventsFilter::EventType::CHANGED}); + */ + RCLCPP_PUBLIC + ParameterEventsFilter( + rcl_interfaces::msg::ParameterEvent::SharedPtr event, + const std::vector & names, + const std::vector & types); + + /// Get the result of the filter + /** + * \return A std::vector of all matching parameter changes in this event. + */ + RCLCPP_PUBLIC + const std::vector & get_events() const; + +private: + // access only allowed via const accessor. + std::vector result_; ///< Storage of the resultant vector + rcl_interfaces::msg::ParameterEvent::SharedPtr event_; ///< Keep event in scope +}; + +} // namespace rclcpp + +#endif // RCLCPP__PARAMETER_EVENTS_FILTER_HPP_ diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 60de90e..40b93dc 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -24,6 +24,7 @@ #include "rclcpp/node.hpp" #include "rclcpp/parameter_client.hpp" +#include "rclcpp/parameter_events_filter.hpp" namespace rclcpp diff --git a/rclcpp/src/rclcpp/parameter_events_filter.cpp b/rclcpp/src/rclcpp/parameter_events_filter.cpp new file mode 100644 index 0000000..36ef708 --- /dev/null +++ b/rclcpp/src/rclcpp/parameter_events_filter.cpp @@ -0,0 +1,60 @@ +// Copyright 2017 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 "rclcpp/parameter_events_filter.hpp" + +#include +#include + +using rclcpp::ParameterEventsFilter; +using EventType = rclcpp::ParameterEventsFilter::EventType; +using EventPair = rclcpp::ParameterEventsFilter::EventPair; + +ParameterEventsFilter::ParameterEventsFilter( + rcl_interfaces::msg::ParameterEvent::SharedPtr event, + const std::vector & names, + const std::vector & types) +: event_(event) +{ + if (std::find(types.begin(), types.end(), EventType::NEW) != types.end()) { + for (auto & new_parameter : event->new_parameters) { + if (std::find(names.begin(), names.end(), new_parameter.name) != names.end()) { + result_.push_back( + EventPair(EventType::NEW, &new_parameter)); + } + } + } + if (std::find(types.begin(), types.end(), EventType::CHANGED) != types.end()) { + for (auto & changed_parameter : event->changed_parameters) { + if (std::find(names.begin(), names.end(), changed_parameter.name) != names.end()) { + result_.push_back( + EventPair(EventType::CHANGED, &changed_parameter)); + } + } + } + if (std::find(types.begin(), types.end(), EventType::DELETED) != types.end()) { + for (auto & deleted_parameter : event->deleted_parameters) { + if (std::find(names.begin(), names.end(), deleted_parameter.name) != names.end()) { + result_.push_back( + EventPair(EventType::DELETED, &deleted_parameter)); + } + } + } +} + +const std::vector & +ParameterEventsFilter::get_events() const +{ + return result_; +} diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 4c3ff81..6ea9a0a 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -13,7 +13,9 @@ // limitations under the License. #include +#include #include +#include #include "builtin_interfaces/msg/time.hpp" @@ -167,39 +169,30 @@ void TimeSource::clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg) void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) { - for (auto & new_parameter : event->new_parameters) { - if (new_parameter.value.type == rclcpp::parameter::ParameterType::PARAMETER_BOOL) { - if (new_parameter.name == "use_sim_time") { - bool value = new_parameter.value.bool_value; - if (value) { - parameter_state_ = SET_TRUE; - enable_ros_time(); - } else { - parameter_state_ = SET_FALSE; - disable_ros_time(); - } - } + // Filter for only 'use_sim_time' being added or changed. + rclcpp::ParameterEventsFilter filter(event, {"use_sim_time"}, + {rclcpp::ParameterEventsFilter::EventType::NEW, + rclcpp::ParameterEventsFilter::EventType::CHANGED}); + for (auto & it : filter.get_events()) { + if (it.second->value.type != parameter::ParameterType::PARAMETER_BOOL) { + RCUTILS_LOG_ERROR("use_sim_time parameter set to something besides a bool"); + continue; + } + if (it.second->value.bool_value) { + parameter_state_ = SET_TRUE; + enable_ros_time(); + } else { + parameter_state_ = SET_FALSE; + disable_ros_time(); } } - for (auto & changed_parameter : event->changed_parameters) { - if (changed_parameter.value.type == rclcpp::parameter::ParameterType::PARAMETER_BOOL) { - if (changed_parameter.name == "use_sim_time") { - bool value = changed_parameter.value.bool_value; - if (value) { - parameter_state_ = SET_TRUE; - enable_ros_time(); - } else { - parameter_state_ = SET_FALSE; - disable_ros_time(); - } - } - } - } - for (auto & deleted_parameter : event->deleted_parameters) { - if (deleted_parameter.name == "use_sim_time") { - // If the parameter is deleted mark it as unset but dont' change state. - parameter_state_ = UNSET; - } + // Handle the case that use_sim_time was deleted. + rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"}, + {rclcpp::ParameterEventsFilter::EventType::DELETED}); + for (auto & it : deleted.get_events()) { + (void) it; // if there is a match it's already matched, don't bother reading it. + // If the parameter is deleted mark it as unset but dont' change state. + parameter_state_ = UNSET; } } diff --git a/rclcpp/test/test_parameter_events_filter.cpp b/rclcpp/test/test_parameter_events_filter.cpp new file mode 100644 index 0000000..a497a28 --- /dev/null +++ b/rclcpp/test/test_parameter_events_filter.cpp @@ -0,0 +1,205 @@ +// Copyright 2017 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 "rclcpp/exceptions.hpp" +#include "rclcpp/parameter_events_filter.hpp" + +#include "rcl_interfaces/msg/parameter_event.hpp" + +class TestParameterEventFilter : public ::testing::Test +{ +protected: + void SetUp() + { + empty = std::make_shared(); + full = std::make_shared(); + multiple = std::make_shared(); + np = std::make_shared(); + cp = std::make_shared(); + dp = std::make_shared(); + + rcl_interfaces::msg::Parameter p; + p.name = "new"; + p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + p.value.integer_value = 1; + full->new_parameters.push_back(p); + np->new_parameters.push_back(p); + multiple->new_parameters.push_back(p); + + p.name = "new2"; + p.value.integer_value = 2; + multiple->new_parameters.push_back(p); + + p.name = "changed"; + p.value.integer_value = 1; + full->changed_parameters.push_back(p); + cp->changed_parameters.push_back(p); + + p.name = "deleted"; + p.value.integer_value = 1; + full->deleted_parameters.push_back(p); + dp->changed_parameters.push_back(p); + } + + rcl_interfaces::msg::ParameterEvent::SharedPtr empty; + rcl_interfaces::msg::ParameterEvent::SharedPtr full; + rcl_interfaces::msg::ParameterEvent::SharedPtr multiple; + rcl_interfaces::msg::ParameterEvent::SharedPtr np; + rcl_interfaces::msg::ParameterEvent::SharedPtr cp; + rcl_interfaces::msg::ParameterEvent::SharedPtr dp; + + rclcpp::ParameterEventsFilter::EventType nt = rclcpp::ParameterEventsFilter::EventType::NEW; + rclcpp::ParameterEventsFilter::EventType ct = rclcpp::ParameterEventsFilter::EventType::CHANGED; + rclcpp::ParameterEventsFilter::EventType dt = rclcpp::ParameterEventsFilter::EventType::DELETED; +}; + +/* + Testing filters. + */ +TEST_F(TestParameterEventFilter, full_by_type) { + auto res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(3u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {nt, ct}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {nt, dt}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {ct, dt}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {nt}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {ct}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {dt}); + EXPECT_EQ(1u, res.get_events().size()); +} + +TEST_F(TestParameterEventFilter, full_by_name) { + auto res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(3u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed"}, + {nt, ct, dt}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"changed"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"deleted"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); +} + +TEST_F(TestParameterEventFilter, empty) { + auto res = rclcpp::ParameterEventsFilter( + empty, + {"new", "changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(0u, res.get_events().size()); +} + +TEST_F(TestParameterEventFilter, singular) { + auto res = rclcpp::ParameterEventsFilter( + np, + {"new", "changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + cp, + {"new", "changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + dp, + {"new", "changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); +} + +TEST_F(TestParameterEventFilter, multiple) { + auto res = rclcpp::ParameterEventsFilter( + multiple, + {"new", "new2"}, + {nt, ct, dt}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + multiple, + {"new2"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + multiple, + {"new", "new2"}, + {ct, dt}); + EXPECT_EQ(0u, res.get_events().size()); +} + +TEST_F(TestParameterEventFilter, validate_data) { + auto res = rclcpp::ParameterEventsFilter( + multiple, + {"new", "new2"}, + {nt, ct, dt}); + EXPECT_EQ(2u, res.get_events().size()); + EXPECT_EQ(nt, res.get_events()[0].first); + EXPECT_EQ(nt, res.get_events()[1].first); + EXPECT_EQ(1, res.get_events()[0].second->value.integer_value); + EXPECT_EQ(2, res.get_events()[1].second->value.integer_value); +}