diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 5b6db7f025..86369b126e 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -184,10 +184,12 @@ class AnySubscriptionCallback } else if (const_shared_ptr_with_info_callback_) { const_shared_ptr_with_info_callback_(message, message_info); } else { - if (unique_ptr_callback_ || unique_ptr_with_info_callback_ || + if ( + unique_ptr_callback_ || unique_ptr_with_info_callback_ || shared_ptr_callback_ || shared_ptr_with_info_callback_) { - throw std::runtime_error("unexpected dispatch_intra_process const shared " + throw std::runtime_error( + "unexpected dispatch_intra_process const shared " "message call with no const shared_ptr callback"); } else { throw std::runtime_error("unexpected message without any callback set"); @@ -209,7 +211,8 @@ class AnySubscriptionCallback } else if (unique_ptr_with_info_callback_) { unique_ptr_with_info_callback_(std::move(message), message_info); } else if (const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_) { - throw std::runtime_error("unexpected dispatch_intra_process unique message call" + throw std::runtime_error( + "unexpected dispatch_intra_process unique message call" " with const shared_ptr callback"); } else { throw std::runtime_error("unexpected message without any callback set"); diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index bee628045f..14a9f74128 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -95,16 +95,17 @@ class MessageMemoryStrategy rclcpp::exceptions::throw_from_rcl_error(ret); } - auto serialized_msg = std::shared_ptr(msg, - [](rmw_serialized_message_t * msg) { - auto ret = rmw_serialized_message_fini(msg); - delete msg; - if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy serialized message: %s", rcl_get_error_string().str); - } - }); + auto serialized_msg = std::shared_ptr( + msg, + [](rmw_serialized_message_t * msg) { + auto ret = rmw_serialized_message_fini(msg); + delete msg; + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to destroy serialized message: %s", rcl_get_error_string().str); + } + }); return serialized_msg; } diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 55afcbc3d2..f5a1dbc9b6 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -404,9 +404,7 @@ Node::get_parameter_or_set( bool got_parameter = get_parameter(sub_name, value); if (!got_parameter) { - this->set_parameters({ - rclcpp::Parameter(sub_name, alternative_value), - }); + this->set_parameters({rclcpp::Parameter(sub_name, alternative_value), }); value = alternative_value; } } diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 4752e023b9..a0bd342cc9 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -70,12 +70,12 @@ class Publisher : public PublisherBase allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); if (event_callbacks.deadline_callback) { - this->add_event_handler(event_callbacks.deadline_callback, + this->add_event_handler( + event_callbacks.deadline_callback, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); } if (event_callbacks.liveliness_callback) { - this->add_event_handler(event_callbacks.liveliness_callback, - RCL_PUBLISHER_LIVELINESS_LOST); + this->add_event_handler(event_callbacks.liveliness_callback, RCL_PUBLISHER_LIVELINESS_LOST); } } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 6b394a29e4..e963e856ff 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -100,11 +100,13 @@ class Subscription : public SubscriptionBase message_memory_strategy_(memory_strategy) { if (event_callbacks.deadline_callback) { - this->add_event_handler(event_callbacks.deadline_callback, + this->add_event_handler( + event_callbacks.deadline_callback, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); } if (event_callbacks.liveliness_callback) { - this->add_event_handler(event_callbacks.liveliness_callback, + this->add_event_handler( + event_callbacks.liveliness_callback, RCL_SUBSCRIPTION_LIVELINESS_CHANGED); } } diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 0117732264..a0e02cc362 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -119,8 +119,9 @@ Clock::create_jump_callback( } // Try to add the jump callback to the clock - rcl_ret_t ret = rcl_clock_add_jump_callback(&rcl_clock_, threshold, - Clock::on_time_jump, handler.get()); + rcl_ret_t ret = rcl_clock_add_jump_callback( + &rcl_clock_, threshold, Clock::on_time_jump, + handler.get()); if (RCL_RET_OK != ret) { exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback"); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 48d087f7a4..11347881b3 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -137,7 +137,8 @@ NodeGraph::get_node_names() const std::vector nodes; auto names_and_namespaces = get_node_names_and_namespaces(); - std::transform(names_and_namespaces.begin(), + std::transform( + names_and_namespaces.begin(), names_and_namespaces.end(), std::back_inserter(nodes), [](std::pair nns) { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 8dd064e943..73978821ee 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -242,9 +242,7 @@ __check_parameter_value_in_range( if (!descriptor.floating_point_range.empty() && value.get_type() == rclcpp::PARAMETER_DOUBLE) { double v = value.get(); auto fp_range = descriptor.floating_point_range.at(0); - if (__are_doubles_equal(v, fp_range.from_value) || - __are_doubles_equal(v, fp_range.to_value)) - { + if (__are_doubles_equal(v, fp_range.from_value) || __are_doubles_equal(v, fp_range.to_value)) { return result; } if ((v < fp_range.from_value) || (v > fp_range.to_value)) { @@ -831,25 +829,27 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 bool get_all = (prefixes.size() == 0) && ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || (static_cast(std::count(kv.first.begin(), kv.first.end(), *separator)) < depth)); - bool prefix_matches = std::any_of(prefixes.cbegin(), prefixes.cend(), - [&kv, &depth, &separator](const std::string & prefix) { - if (kv.first == prefix) { - return true; - } else if (kv.first.find(prefix + separator) == 0) { - size_t length = prefix.length(); - std::string substr = kv.first.substr(length); - // Cast as unsigned integer to avoid warning - return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || - (static_cast(std::count(substr.begin(), substr.end(), *separator)) < depth); - } - return false; - }); + bool prefix_matches = std::any_of( + prefixes.cbegin(), prefixes.cend(), + [&kv, &depth, &separator](const std::string & prefix) { + if (kv.first == prefix) { + return true; + } else if (kv.first.find(prefix + separator) == 0) { + size_t length = prefix.length(); + std::string substr = kv.first.substr(length); + // Cast as unsigned integer to avoid warning + return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || + (static_cast(std::count(substr.begin(), substr.end(), *separator)) < depth); + } + return false; + }); if (get_all || prefix_matches) { result.names.push_back(kv.first); size_t last_separator = kv.first.find_last_of(separator); if (std::string::npos != last_separator) { std::string prefix = kv.first.substr(0, last_separator); - if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == + if ( + std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == result.prefixes.cend()) { result.prefixes.push_back(prefix); diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index c49f5c2ab2..689d7eaed3 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -71,10 +71,11 @@ AsyncParametersClient::AsyncParametersClient( node_services_interface->add_client(set_parameters_base, nullptr); set_parameters_atomically_client_ = - Client::make_shared(node_base_interface.get(), - node_graph_interface, - remote_node_name_ + "/" + parameter_service_names::set_parameters_atomically, - options); + Client::make_shared( + node_base_interface.get(), + node_graph_interface, + remote_node_name_ + "/" + parameter_service_names::set_parameters_atomically, + options); auto set_parameters_atomically_base = std::dynamic_pointer_cast( set_parameters_atomically_client_); node_services_interface->add_client(set_parameters_atomically_base, nullptr); @@ -150,8 +151,7 @@ AsyncParametersClient::get_parameters( rcl_interfaces::msg::Parameter parameter; parameter.name = request->names[i]; parameter.value = pvalue; - parameters.push_back(rclcpp::Parameter::from_parameter_msg( - parameter)); + parameters.push_back(rclcpp::Parameter::from_parameter_msg(parameter)); } promise_result->set_value(parameters); @@ -211,10 +211,9 @@ AsyncParametersClient::set_parameters( auto request = std::make_shared(); - std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters), - [](rclcpp::Parameter p) { - return p.to_parameter_msg(); - } + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(request->parameters), + [](rclcpp::Parameter p) {return p.to_parameter_msg();} ); set_parameters_client_->async_send_request( @@ -245,10 +244,9 @@ AsyncParametersClient::set_parameters_atomically( auto request = std::make_shared(); - std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters), - [](rclcpp::Parameter p) { - return p.to_parameter_msg(); - } + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(request->parameters), + [](rclcpp::Parameter p) {return p.to_parameter_msg();} ); set_parameters_atomically_client_->async_send_request( @@ -411,8 +409,10 @@ SyncParametersClient::get_parameters(const std::vector & parameter_ { auto f = async_parameters_client_->get_parameters(parameter_names); using rclcpp::executors::spin_node_until_future_complete; - if (spin_node_until_future_complete(*executor_, node_base_interface_, f) == - rclcpp::executor::FutureReturnCode::SUCCESS) + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, + f) == rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); } @@ -435,8 +435,10 @@ SyncParametersClient::get_parameter_types(const std::vector & param auto f = async_parameters_client_->get_parameter_types(parameter_names); using rclcpp::executors::spin_node_until_future_complete; - if (spin_node_until_future_complete(*executor_, node_base_interface_, f) == - rclcpp::executor::FutureReturnCode::SUCCESS) + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, + f) == rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); } @@ -450,8 +452,10 @@ SyncParametersClient::set_parameters( auto f = async_parameters_client_->set_parameters(parameters); using rclcpp::executors::spin_node_until_future_complete; - if (spin_node_until_future_complete(*executor_, node_base_interface_, f) == - rclcpp::executor::FutureReturnCode::SUCCESS) + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, + f) == rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); } @@ -465,8 +469,10 @@ SyncParametersClient::set_parameters_atomically( auto f = async_parameters_client_->set_parameters_atomically(parameters); using rclcpp::executors::spin_node_until_future_complete; - if (spin_node_until_future_complete(*executor_, node_base_interface_, f) == - rclcpp::executor::FutureReturnCode::SUCCESS) + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, + f) == rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); } @@ -482,8 +488,10 @@ SyncParametersClient::list_parameters( auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); using rclcpp::executors::spin_node_until_future_complete; - if (spin_node_until_future_complete(*executor_, node_base_interface_, f) == - rclcpp::executor::FutureReturnCode::SUCCESS) + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, + f) == rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); } diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 6cc25e78a9..f1f9166962 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -59,10 +59,11 @@ ParameterService::ParameterService( { try { auto types = node_params->get_parameter_types(request->names); - std::transform(types.cbegin(), types.cend(), - std::back_inserter(response->types), [](const uint8_t & type) { - return static_cast(type); - }); + std::transform( + types.cbegin(), types.cend(), + std::back_inserter(response->types), [](const uint8_t & type) { + return static_cast(type); + }); } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what()); } @@ -103,11 +104,12 @@ ParameterService::ParameterService( std::shared_ptr response) { std::vector pvariants; - std::transform(request->parameters.cbegin(), request->parameters.cend(), - std::back_inserter(pvariants), - [](const rcl_interfaces::msg::Parameter & p) { - return rclcpp::Parameter::from_parameter_msg(p); - }); + std::transform( + request->parameters.cbegin(), request->parameters.cend(), + std::back_inserter(pvariants), + [](const rcl_interfaces::msg::Parameter & p) { + return rclcpp::Parameter::from_parameter_msg(p); + }); try { auto result = node_params->set_parameters_atomically(pvariants); response->result = result; diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 7165afc8bf..bde1c50d93 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -100,7 +100,8 @@ void TimeSource::attachNode( } else { // TODO(wjwwood): use set_on_parameters_set_callback to catch the type mismatch, // before the use_sim_time parameter can ever be set to an invalid value - RCLCPP_ERROR(logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'", + RCLCPP_ERROR( + logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'", rclcpp::to_string(use_sim_time_param.get_type()).c_str()); } @@ -154,7 +155,8 @@ void TimeSource::detachClock(std::shared_ptr clock) TimeSource::~TimeSource() { - if (node_base_ || node_topics_ || node_graph_ || node_services_ || + if ( + node_base_ || node_topics_ || node_graph_ || node_services_ || node_logging_ || node_clock_ || node_parameters_) { this->detachNode(); diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index bcb024501d..8291a12f5d 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -55,7 +55,8 @@ TimerBase::TimerBase( *timer_handle_.get() = rcl_get_zero_initialized_timer(); rcl_clock_t * clock_handle = clock_->get_clock_handle(); - if (rcl_timer_init( + if ( + rcl_timer_init( timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr, rcl_get_default_allocator()) != RCL_RET_OK) { @@ -110,12 +111,14 @@ std::chrono::nanoseconds TimerBase::time_until_trigger() { int64_t time_until_next_call = 0; - if (rcl_timer_get_time_until_next_call(timer_handle_.get(), - &time_until_next_call) != RCL_RET_OK) + if ( + rcl_timer_get_time_until_next_call( + timer_handle_.get(), + &time_until_next_call) != RCL_RET_OK) { throw std::runtime_error( - std::string("Timer could not get time until next call: ") + - rcl_get_error_string().str); + std::string( + "Timer could not get time until next call: ") + rcl_get_error_string().str); } return std::chrono::nanoseconds(time_until_next_call); } diff --git a/rclcpp/test/test_client.cpp b/rclcpp/test/test_client.cpp index 8fd1b5657b..fb3fefdb47 100644 --- a/rclcpp/test/test_client.cpp +++ b/rclcpp/test/test_client.cpp @@ -75,7 +75,8 @@ TEST_F(TestClient, construction_and_destruction) { } { - ASSERT_THROW({ + ASSERT_THROW( + { auto client = node->create_client("invalid_service?"); }, rclcpp::exceptions::InvalidServiceNameError); } @@ -92,7 +93,8 @@ TEST_F(TestClient, construction_with_free_function) { nullptr); } { - ASSERT_THROW({ + ASSERT_THROW( + { auto client = rclcpp::create_client( node->get_node_base_interface(), node->get_node_graph_interface(), @@ -115,7 +117,8 @@ TEST_F(TestClientSub, construction_and_destruction) { } { - ASSERT_THROW({ + ASSERT_THROW( + { auto client = node->create_client("invalid_service?"); }, rclcpp::exceptions::InvalidServiceNameError); } diff --git a/rclcpp/test/test_expand_topic_or_service_name.cpp b/rclcpp/test/test_expand_topic_or_service_name.cpp index 4553b19c43..98c10609b4 100644 --- a/rclcpp/test/test_expand_topic_or_service_name.cpp +++ b/rclcpp/test/test_expand_topic_or_service_name.cpp @@ -33,39 +33,45 @@ TEST(TestExpandTopicOrServiceName, normal) { TEST(TestExpandTopicOrServiceName, exceptions) { using rclcpp::expand_topic_or_service_name; { - ASSERT_THROW({ + ASSERT_THROW( + { expand_topic_or_service_name("chatter", "invalid_node?", "/ns"); }, rclcpp::exceptions::InvalidNodeNameError); } { - ASSERT_THROW({ + ASSERT_THROW( + { expand_topic_or_service_name("chatter", "node", "/invalid_ns?"); }, rclcpp::exceptions::InvalidNamespaceError); } { - ASSERT_THROW({ + ASSERT_THROW( + { expand_topic_or_service_name("chatter/42invalid", "node", "/ns"); }, rclcpp::exceptions::InvalidTopicNameError); } { - ASSERT_THROW({ + ASSERT_THROW( + { // this one will only fail on the "full" topic name validation check expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns"); }, rclcpp::exceptions::InvalidTopicNameError); } { - ASSERT_THROW({ + ASSERT_THROW( + { // is_service = true expand_topic_or_service_name("chatter/42invalid", "node", "/ns", true); }, rclcpp::exceptions::InvalidServiceNameError); } { - ASSERT_THROW({ + ASSERT_THROW( + { // is_service = true // this one will only fail on the "full" topic name validation check expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns", true); diff --git a/rclcpp/test/test_function_traits.cpp b/rclcpp/test/test_function_traits.cpp index d166ed252e..a9e6e8d261 100644 --- a/rclcpp/test/test_function_traits.cpp +++ b/rclcpp/test/test_function_traits.cpp @@ -742,8 +742,9 @@ class TestMember : public ::testing::Test TEST_F GTest macro. */ TEST_F(TestMember, bind_member_functor) { - auto bind_member_functor = std::bind(&TestMember::MemberFunctor, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3); + auto bind_member_functor = std::bind( + &TestMember::MemberFunctor, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3); static_assert( rclcpp::function_traits::check_arguments("invalid_node?", "/ns"); }, rclcpp::exceptions::InvalidNodeNameError); } { - ASSERT_THROW({ + ASSERT_THROW( + { std::make_shared("my_node", "/invalid_ns?"); }, rclcpp::exceptions::InvalidNamespaceError); } @@ -106,8 +108,9 @@ TEST_F(TestNode, get_name_and_namespace) { auto node3 = std::make_shared("my_node3", "/ns2"); auto node4 = std::make_shared("my_node4", "my/ns3"); auto names_and_namespaces = node1->get_node_names(); - auto name_namespace_set = std::unordered_set(names_and_namespaces.begin(), - names_and_namespaces.end()); + auto name_namespace_set = std::unordered_set( + names_and_namespaces.begin(), + names_and_namespaces.end()); std::function Set_Contains = [&](std::string string_key) { return name_namespace_set.find(string_key) != name_namespace_set.end(); @@ -172,7 +175,8 @@ TEST_F(TestNode, subnode_get_name_and_namespace) { } { auto node = std::make_shared("my_node"); - ASSERT_THROW({ + ASSERT_THROW( + { auto subnode = node->create_sub_node("/sub_ns"); }, rclcpp::exceptions::NameValidationError); } @@ -182,60 +186,70 @@ TEST_F(TestNode, subnode_get_name_and_namespace) { */ TEST_F(TestNode, subnode_construction_and_destruction) { { - ASSERT_NO_THROW({ + ASSERT_NO_THROW( + { auto node = std::make_shared("my_node", "ns"); auto subnode = node->create_sub_node("sub_ns"); }); } { - ASSERT_THROW({ + ASSERT_THROW( + { auto node = std::make_shared("my_node", "ns"); auto subnode = node->create_sub_node("invalid_ns?"); }, rclcpp::exceptions::InvalidNamespaceError); } { - ASSERT_THROW({ + ASSERT_THROW( + { auto node = std::make_shared("my_node", "ns/"); }, rclcpp::exceptions::InvalidNamespaceError); } { - ASSERT_THROW({ + ASSERT_THROW( + { auto node = std::make_shared("my_node", "ns/"); auto subnode = node->create_sub_node("/sub_ns"); }, rclcpp::exceptions::InvalidNamespaceError); } { - ASSERT_THROW({ + ASSERT_THROW( + { auto node = std::make_shared("my_node", "ns"); auto subnode = node->create_sub_node("/sub_ns"); }, rclcpp::exceptions::NameValidationError); } { - ASSERT_THROW({ + ASSERT_THROW( + { auto node = std::make_shared("my_node", "ns"); auto subnode = node->create_sub_node("~sub_ns"); }, rclcpp::exceptions::InvalidNamespaceError); } { - ASSERT_THROW({ + ASSERT_THROW( + { auto node = std::make_shared("my_node", "/ns"); auto subnode = node->create_sub_node("invalid_ns?"); }, rclcpp::exceptions::InvalidNamespaceError); } { - ASSERT_NO_THROW({ + ASSERT_NO_THROW( + { auto node = std::make_shared("my_node", "/ns"); auto subnode = node->create_sub_node("sub_ns"); }); } { - ASSERT_THROW({ + ASSERT_THROW( + { auto node = std::make_shared("my_node", "/ns"); auto subnode = node->create_sub_node("/sub_ns"); }, rclcpp::exceptions::NameValidationError); } { - ASSERT_THROW({ + ASSERT_THROW( + { auto node = std::make_shared("my_node", "/ns"); auto subnode = node->create_sub_node("~sub_ns"); }, rclcpp::exceptions::InvalidNamespaceError); @@ -442,7 +456,8 @@ TEST_F(TestNode, test_registering_multiple_callbacks_api) { TEST_F(TestNode, declare_parameter_with_overrides) { // test cases with overrides rclcpp::NodeOptions no; - no.parameter_overrides({ + no.parameter_overrides( + { {"parameter_no_default", 42}, {"parameter_no_default_set", 42}, {"parameter_no_default_set_cvref", 42}, @@ -571,7 +586,8 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) { { // with namespace, defaults, no custom descriptors, no initial int64_t bigger_than_int = INT64_MAX - 42; - auto values = node->declare_parameters("namespace1", { + auto values = node->declare_parameters( + "namespace1", { {"parameter_a", 42}, {"parameter_b", 256}, {"parameter_c", bigger_than_int}, @@ -584,7 +600,8 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) { } { // without namespace, defaults, no custom descriptors, no initial - auto values = node->declare_parameters("", { + auto values = node->declare_parameters( + "", { {"parameter_without_ns_a", 42}, {"parameter_without_ns_b", 256}, }); @@ -597,7 +614,8 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) { // with namespace, defaults, custom descriptors, no initial rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.read_only = true; - auto values = node->declare_parameters("namespace2", { + auto values = node->declare_parameters( + "namespace2", { {"parameter_a", {42, descriptor}}, {"parameter_b", {256, descriptor}}, }); @@ -611,7 +629,8 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) { // without namespace, defaults, custom descriptors, no initial rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.read_only = true; - auto values = node->declare_parameters("", { + auto values = node->declare_parameters( + "", { {"parameter_without_ns_c", {42, descriptor}}, {"parameter_without_ns_d", {256, descriptor}}, }); @@ -1156,7 +1175,8 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) { rclcpp::NodeOptions no; - no.parameter_overrides({ + no.parameter_overrides( + { {"parameter_with_override", 30}, }); no.allow_undeclared_parameters(true); @@ -1203,7 +1223,8 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { node->declare_parameter(name2, true); node->declare_parameter(name3, "blue"); - auto rets = node->set_parameters({ + auto rets = node->set_parameters( + { {name1, 2}, {name2, false}, {name3, "red"}, @@ -1218,7 +1239,8 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { auto name = "parameter"_unq; node->declare_parameter(name, 1); - auto rets = node->set_parameters({ + auto rets = node->set_parameters( + { {name, 42}, {name, 2}, }); @@ -1237,7 +1259,8 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { EXPECT_THROW( { - node->set_parameters({ + node->set_parameters( + { {name1, 2}, {name2, "not declared :("}, {name3, 101}, @@ -1274,7 +1297,8 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { }; node->set_on_parameters_set_callback(on_set_parameters); - auto rets = node->set_parameters({ + auto rets = node->set_parameters( + { {name1, 2}, {name2, false}, {name3, "red"}, @@ -1334,7 +1358,8 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_allowed) { EXPECT_TRUE(node->has_parameter(name2)); EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); - auto rets = node->set_parameters({ + auto rets = node->set_parameters( + { rclcpp::Parameter(name1, 43), rclcpp::Parameter(name2, "other"), }); @@ -1350,7 +1375,8 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_allowed) { EXPECT_FALSE(node->has_parameter(name1)); EXPECT_FALSE(node->has_parameter(name2)); - auto rets = node->set_parameters({ + auto rets = node->set_parameters( + { rclcpp::Parameter(name1, 42), rclcpp::Parameter(name2, "test"), }); @@ -1373,7 +1399,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { node->declare_parameter(name2, true); node->declare_parameter(name3, "blue"); - auto ret = node->set_parameters_atomically({ + auto ret = node->set_parameters_atomically( + { {name1, 2}, {name2, false}, {name3, "red"}, @@ -1388,7 +1415,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { auto name = "parameter"_unq; node->declare_parameter(name, 1); - auto ret = node->set_parameters_atomically({ + auto ret = node->set_parameters_atomically( + { {name, 42}, {name, 2}, }); @@ -1407,7 +1435,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { EXPECT_THROW( { - node->set_parameters_atomically({ + node->set_parameters_atomically( + { {name1, 2}, {name2, "not declared :("}, {name3, 101}, @@ -1445,7 +1474,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { }; node->set_on_parameters_set_callback(on_set_parameters); - auto ret = node->set_parameters_atomically({ + auto ret = node->set_parameters_atomically( + { {name1, 2}, {name2, false}, // should fail to be set, failing the whole operation {name3, "red"}, @@ -1502,7 +1532,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { EXPECT_TRUE(node->has_parameter(name2)); EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); - auto ret = node->set_parameters_atomically({ + auto ret = node->set_parameters_atomically( + { rclcpp::Parameter(name1, 43), rclcpp::Parameter(name2, "other"), }); @@ -1518,7 +1549,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { EXPECT_FALSE(node->has_parameter(name1)); EXPECT_FALSE(node->has_parameter(name2)); - auto ret = node->set_parameters_atomically({ + auto ret = node->set_parameters_atomically( + { rclcpp::Parameter(name1, 42), rclcpp::Parameter(name2, "test"), }); @@ -1553,7 +1585,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { }; node->set_on_parameters_set_callback(on_set_parameters); - auto ret = node->set_parameters_atomically({ + auto ret = node->set_parameters_atomically( + { rclcpp::Parameter(name1, 43), rclcpp::Parameter(name2, true), // this would cause implicit declaration rclcpp::Parameter(name3, "other"), // this set should fail, and fail the whole operation @@ -1933,7 +1966,8 @@ TEST_F(TestNode, describe_parameter_undeclared_parameters_not_allowed) { auto name = "parameter"_unq; { - EXPECT_THROW({ + EXPECT_THROW( + { node->describe_parameter(name); }, rclcpp::exceptions::ParameterNotDeclaredException); } @@ -2013,7 +2047,8 @@ TEST_F(TestNode, describe_parameters_undeclared_parameters_not_allowed) { auto name = "parameter"_unq; { - EXPECT_THROW({ + EXPECT_THROW( + { node->describe_parameters({name}); }, rclcpp::exceptions::ParameterNotDeclaredException); } @@ -2026,7 +2061,8 @@ TEST_F(TestNode, describe_parameters_undeclared_parameters_not_allowed) { node->declare_parameter(name1, 42); { - EXPECT_THROW({ + EXPECT_THROW( + { node->describe_parameters({name1, name2}); }, rclcpp::exceptions::ParameterNotDeclaredException); } @@ -2149,7 +2185,8 @@ TEST_F(TestNode, get_parameter_types_undeclared_parameters_not_allowed) { auto name = "parameter"_unq; { - EXPECT_THROW({ + EXPECT_THROW( + { node->get_parameter_types({name}); }, rclcpp::exceptions::ParameterNotDeclaredException); } @@ -2292,7 +2329,8 @@ TEST_F(TestNode, set_on_parameters_set_callback_set_parameter) { }; EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); - EXPECT_THROW({ + EXPECT_THROW( + { node->set_parameter(rclcpp::Parameter("intparam", 40)); }, rclcpp::exceptions::ParameterModifiedInCallbackException); } @@ -2326,7 +2364,8 @@ TEST_F(TestNode, set_on_parameters_set_callback_declare_parameter) { }; EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); - EXPECT_THROW({ + EXPECT_THROW( + { node->set_parameter(rclcpp::Parameter("intparam", 40)); }, rclcpp::exceptions::ParameterModifiedInCallbackException); } @@ -2360,7 +2399,8 @@ TEST_F(TestNode, set_on_parameters_set_callback_undeclare_parameter) { }; EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); - EXPECT_THROW({ + EXPECT_THROW( + { node->set_parameter(rclcpp::Parameter("intparam", 40)); }, rclcpp::exceptions::ParameterModifiedInCallbackException); } @@ -2401,7 +2441,8 @@ TEST_F(TestNode, set_on_parameters_set_callback_set_on_parameters_set_callback) }; EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); - EXPECT_THROW({ + EXPECT_THROW( + { node->set_parameter(rclcpp::Parameter("intparam", 40)); }, rclcpp::exceptions::ParameterModifiedInCallbackException); } diff --git a/rclcpp/test/test_parameter.cpp b/rclcpp/test/test_parameter.cpp index 01cbc00683..876fbb4e93 100644 --- a/rclcpp/test/test_parameter.cpp +++ b/rclcpp/test/test_parameter.cpp @@ -53,7 +53,8 @@ TEST(TestParameter, not_set_variant) { EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET, not_set_param.value.type); // From parameter message - EXPECT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_NOT_SET, rclcpp::Parameter::from_parameter_msg(not_set_param).get_type()); } @@ -65,7 +66,8 @@ TEST(TestParameter, bool_variant) { EXPECT_EQ("bool", bool_variant_true.get_type_name()); EXPECT_TRUE(bool_variant_true.get_value()); EXPECT_TRUE(bool_variant_true.get_value_message().bool_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, bool_variant_true.get_value_message().type); EXPECT_TRUE(bool_variant_true.as_bool()); @@ -83,7 +85,8 @@ TEST(TestParameter, bool_variant) { rclcpp::Parameter bool_variant_false("bool_param", false); EXPECT_FALSE(bool_variant_false.get_value()); EXPECT_FALSE(bool_variant_false.get_value_message().bool_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, bool_variant_false.get_value_message().type); rcl_interfaces::msg::Parameter bool_param = bool_variant_true.to_parameter_msg(); @@ -99,7 +102,8 @@ TEST(TestParameter, bool_variant) { EXPECT_EQ("bool", from_msg_true.get_type_name()); EXPECT_TRUE(from_msg_true.get_value()); EXPECT_TRUE(from_msg_true.get_value_message().bool_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, bool_variant_false.get_value_message().type); bool_param.value.bool_value = false; @@ -107,7 +111,8 @@ TEST(TestParameter, bool_variant) { rclcpp::Parameter::from_parameter_msg(bool_param); EXPECT_FALSE(from_msg_false.get_value()); EXPECT_FALSE(from_msg_false.get_value_message().bool_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, bool_variant_false.get_value_message().type); } @@ -119,10 +124,12 @@ TEST(TestParameter, integer_variant) { EXPECT_EQ("integer_param", integer_variant.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, integer_variant.get_type()); EXPECT_EQ("integer", integer_variant.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, integer_variant.get_value()); EXPECT_EQ(TEST_VALUE, integer_variant.get_value_message().integer_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, integer_variant.get_value_message().type); EXPECT_EQ(TEST_VALUE, integer_variant.as_int()); @@ -148,10 +155,12 @@ TEST(TestParameter, integer_variant) { EXPECT_EQ("integer_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, from_msg.get_type()); EXPECT_EQ("integer", from_msg.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, from_msg.get_value_message().type); } @@ -163,10 +172,12 @@ TEST(TestParameter, long_integer_variant) { EXPECT_EQ("long_integer_param", long_variant.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, long_variant.get_type()); EXPECT_EQ("integer", long_variant.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, long_variant.get_value()); EXPECT_EQ(TEST_VALUE, long_variant.get_value_message().integer_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, long_variant.get_value_message().type); EXPECT_EQ(TEST_VALUE, long_variant.as_int()); @@ -192,10 +203,12 @@ TEST(TestParameter, long_integer_variant) { EXPECT_EQ("long_integer_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, from_msg.get_type()); EXPECT_EQ("integer", from_msg.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, from_msg.get_value_message().type); } @@ -207,10 +220,12 @@ TEST(TestParameter, float_variant) { EXPECT_EQ("float_param", float_variant.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, float_variant.get_type()); EXPECT_EQ("double", float_variant.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, float_variant.get_value()); EXPECT_EQ(TEST_VALUE, float_variant.get_value_message().double_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, float_variant.get_value_message().type); EXPECT_EQ(TEST_VALUE, float_variant.as_double()); @@ -236,10 +251,12 @@ TEST(TestParameter, float_variant) { EXPECT_EQ("float_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, from_msg.get_type()); EXPECT_EQ("double", from_msg.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, from_msg.get_value_message().type); } @@ -251,10 +268,12 @@ TEST(TestParameter, double_variant) { EXPECT_EQ("double_param", double_variant.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, double_variant.get_type()); EXPECT_EQ("double", double_variant.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, double_variant.get_value()); EXPECT_EQ(TEST_VALUE, double_variant.get_value_message().double_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, double_variant.get_value_message().type); EXPECT_EQ(TEST_VALUE, double_variant.as_double()); @@ -280,10 +299,12 @@ TEST(TestParameter, double_variant) { EXPECT_EQ("double_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, from_msg.get_type()); EXPECT_EQ("double", from_msg.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, from_msg.get_value_message().type); } @@ -295,10 +316,12 @@ TEST(TestParameter, string_variant) { EXPECT_EQ("string_param", string_variant.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING, string_variant.get_type()); EXPECT_EQ("string", string_variant.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, string_variant.get_value()); EXPECT_EQ(TEST_VALUE, string_variant.get_value_message().string_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING, string_variant.get_value_message().type); EXPECT_EQ(TEST_VALUE, string_variant.as_string()); @@ -326,7 +349,8 @@ TEST(TestParameter, string_variant) { EXPECT_EQ("string", from_msg.get_type_name()); EXPECT_EQ(TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().string_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING, from_msg.get_value_message().type); } @@ -338,10 +362,12 @@ TEST(TestParameter, byte_array_variant) { EXPECT_EQ("byte_array_param", byte_array_variant.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BYTE_ARRAY, byte_array_variant.get_type()); EXPECT_EQ("byte_array", byte_array_variant.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, byte_array_variant.get_value()); EXPECT_EQ(TEST_VALUE, byte_array_variant.get_value_message().byte_array_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, byte_array_variant.get_value_message().type); EXPECT_EQ(TEST_VALUE, byte_array_variant.as_byte_array()); @@ -367,10 +393,12 @@ TEST(TestParameter, byte_array_variant) { EXPECT_EQ("byte_array_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BYTE_ARRAY, from_msg.get_type()); EXPECT_EQ("byte_array", from_msg.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().byte_array_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, from_msg.get_value_message().type); } @@ -382,10 +410,12 @@ TEST(TestParameter, bool_array_variant) { EXPECT_EQ("bool_array_param", bool_array_variant.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL_ARRAY, bool_array_variant.get_type()); EXPECT_EQ("bool_array", bool_array_variant.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, bool_array_variant.get_value()); EXPECT_EQ(TEST_VALUE, bool_array_variant.get_value_message().bool_array_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, bool_array_variant.get_value_message().type); EXPECT_EQ(TEST_VALUE, bool_array_variant.as_bool_array()); @@ -411,10 +441,12 @@ TEST(TestParameter, bool_array_variant) { EXPECT_EQ("bool_array_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL_ARRAY, from_msg.get_type()); EXPECT_EQ("bool_array", from_msg.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().bool_array_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, from_msg.get_value_message().type); } @@ -426,10 +458,12 @@ TEST(TestParameter, integer_array_variant) { rclcpp::Parameter integer_array_variant("integer_array_param", TEST_VALUE); EXPECT_EQ("integer_array_param", integer_array_variant.get_name()); - EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, integer_array_variant.get_type()); EXPECT_EQ("integer_array", integer_array_variant.get_type_name()); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, integer_array_variant.get_value_message().type); // No direct comparison of vectors of ints and long ints @@ -464,7 +498,8 @@ TEST(TestParameter, integer_array_variant) { rcl_interfaces::msg::Parameter integer_array_param = integer_array_variant.to_parameter_msg(); EXPECT_EQ("integer_array_param", integer_array_param.name); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, integer_array_param.value.type); param_value = integer_array_param.value.integer_array_value; @@ -489,7 +524,8 @@ TEST(TestParameter, integer_array_variant) { EXPECT_EQ(TEST_VALUE.end(), mismatches.first); EXPECT_EQ(param_value.end(), mismatches.second); - EXPECT_EQ(from_msg.get_value_message().type, + EXPECT_EQ( + from_msg.get_value_message().type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); } @@ -499,12 +535,15 @@ TEST(TestParameter, long_integer_array_variant) { rclcpp::Parameter long_array_variant("long_integer_array_param", TEST_VALUE); EXPECT_EQ("long_integer_array_param", long_array_variant.get_name()); - EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, long_array_variant.get_type()); EXPECT_EQ("integer_array", long_array_variant.get_type_name()); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, long_array_variant.get_value_message().type); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, long_array_variant.get_value()); EXPECT_EQ(TEST_VALUE, long_array_variant.get_value_message().integer_array_value); EXPECT_EQ(TEST_VALUE, long_array_variant.as_integer_array()); @@ -524,7 +563,8 @@ TEST(TestParameter, long_integer_array_variant) { rcl_interfaces::msg::Parameter integer_array_param = long_array_variant.to_parameter_msg(); EXPECT_EQ("long_integer_array_param", integer_array_param.name); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, integer_array_param.value.type); EXPECT_EQ(TEST_VALUE, integer_array_param.value.integer_array_value); @@ -534,10 +574,12 @@ TEST(TestParameter, long_integer_array_variant) { EXPECT_EQ("long_integer_array_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, from_msg.get_type()); EXPECT_EQ("integer_array", from_msg.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_array_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, from_msg.get_value_message().type); } @@ -549,10 +591,12 @@ TEST(TestParameter, float_array_variant) { rclcpp::Parameter float_array_variant("float_array_param", TEST_VALUE); EXPECT_EQ("float_array_param", float_array_variant.get_name()); - EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, float_array_variant.get_type()); EXPECT_EQ("double_array", float_array_variant.get_type_name()); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, float_array_variant.get_value_message().type); // No direct comparison of vectors of floats and doubles @@ -587,7 +631,8 @@ TEST(TestParameter, float_array_variant) { rcl_interfaces::msg::Parameter float_array_param = float_array_variant.to_parameter_msg(); EXPECT_EQ("float_array_param", float_array_param.name); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, float_array_param.value.type); param_value = float_array_param.value.double_array_value; @@ -612,7 +657,8 @@ TEST(TestParameter, float_array_variant) { EXPECT_EQ(TEST_VALUE.end(), mismatches.first); EXPECT_EQ(param_value.end(), mismatches.second); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_value_message().type); } @@ -622,12 +668,15 @@ TEST(TestParameter, double_array_variant) { rclcpp::Parameter double_array_variant("double_array_param", TEST_VALUE); EXPECT_EQ("double_array_param", double_array_variant.get_name()); - EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, double_array_variant.get_type()); EXPECT_EQ("double_array", double_array_variant.get_type_name()); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, double_array_variant.get_value_message().type); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, double_array_variant.get_value()); EXPECT_EQ(TEST_VALUE, double_array_variant.get_value_message().double_array_value); EXPECT_EQ(TEST_VALUE, double_array_variant.as_double_array()); @@ -647,7 +696,8 @@ TEST(TestParameter, double_array_variant) { rcl_interfaces::msg::Parameter double_array_param = double_array_variant.to_parameter_msg(); EXPECT_EQ("double_array_param", double_array_param.name); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, double_array_param.value.type); EXPECT_EQ(TEST_VALUE, double_array_param.value.double_array_value); @@ -657,10 +707,12 @@ TEST(TestParameter, double_array_variant) { EXPECT_EQ("double_array_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_type()); EXPECT_EQ("double_array", from_msg.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_array_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_value_message().type); } @@ -670,13 +722,16 @@ TEST(TestParameter, string_array_variant) { // Direct instantiation rclcpp::Parameter string_array_variant("string_array_param", TEST_VALUE); EXPECT_EQ("string_array_param", string_array_variant.get_name()); - EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING_ARRAY, + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_STRING_ARRAY, string_array_variant.get_type()); EXPECT_EQ("string_array", string_array_variant.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, string_array_variant.get_value()); EXPECT_EQ(TEST_VALUE, string_array_variant.get_value_message().string_array_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, string_array_variant.get_value_message().type); EXPECT_EQ(TEST_VALUE, string_array_variant.as_string_array()); @@ -693,7 +748,8 @@ TEST(TestParameter, string_array_variant) { rcl_interfaces::msg::Parameter string_array_param = string_array_variant.to_parameter_msg(); EXPECT_EQ("string_array_param", string_array_param.name); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, string_array_param.value.type); EXPECT_EQ(TEST_VALUE, string_array_param.value.string_array_value); @@ -703,9 +759,11 @@ TEST(TestParameter, string_array_variant) { EXPECT_EQ("string_array_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING_ARRAY, from_msg.get_type()); EXPECT_EQ("string_array", from_msg.get_type_name()); - EXPECT_EQ(TEST_VALUE, + EXPECT_EQ( + TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().string_array_value); - EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, from_msg.get_value_message().type); } diff --git a/rclcpp/test/test_parameter_client.cpp b/rclcpp/test/test_parameter_client.cpp index baae1ec1cc..2630353ffa 100644 --- a/rclcpp/test/test_parameter_client.cpp +++ b/rclcpp/test/test_parameter_client.cpp @@ -67,7 +67,8 @@ TEST_F(TestParameterClient, async_construction_and_destruction) { } { - ASSERT_THROW({ + ASSERT_THROW( + { std::make_shared(node, "invalid_remote_node?"); }, rclcpp::exceptions::InvalidServiceNameError); } @@ -100,7 +101,8 @@ TEST_F(TestParameterClient, sync_construction_and_destruction) { } { - ASSERT_THROW({ + ASSERT_THROW( + { std::make_shared(node, "invalid_remote_node?"); }, rclcpp::exceptions::InvalidServiceNameError); } diff --git a/rclcpp/test/test_publisher.cpp b/rclcpp/test/test_publisher.cpp index 09cf824b7f..3794b5e288 100644 --- a/rclcpp/test/test_publisher.cpp +++ b/rclcpp/test/test_publisher.cpp @@ -98,7 +98,8 @@ TEST_F(TestPublisher, construction_and_destruction) { } { - ASSERT_THROW({ + ASSERT_THROW( + { auto publisher = node->create_publisher("invalid_topic?", 42); }, rclcpp::exceptions::InvalidTopicNameError); } @@ -233,7 +234,8 @@ TEST_F(TestPublisherSub, construction_and_destruction) { } { - ASSERT_THROW({ + ASSERT_THROW( + { auto publisher = subnode->create_publisher("invalid_topic?", 42); }, rclcpp::exceptions::InvalidTopicNameError); } diff --git a/rclcpp/test/test_serialized_message_allocator.cpp b/rclcpp/test/test_serialized_message_allocator.cpp index 66f8939e1f..bc177722e4 100644 --- a/rclcpp/test/test_serialized_message_allocator.cpp +++ b/rclcpp/test/test_serialized_message_allocator.cpp @@ -56,8 +56,9 @@ TEST(TestSerializedMessageAllocator, borrow_from_subscription) { auto node = std::make_shared("test_serialized_message_allocator_node"); std::shared_ptr sub = - node->create_subscription("~/dummy_topic", 10, - [](std::shared_ptr test_msg) {(void) test_msg;}); + node->create_subscription( + "~/dummy_topic", 10, + [](std::shared_ptr test_msg) {(void) test_msg;}); auto msg0 = sub->create_serialized_message(); EXPECT_EQ(0u, msg0->buffer_capacity); diff --git a/rclcpp/test/test_service.cpp b/rclcpp/test/test_service.cpp index d78f6f3ddd..9ab90e1208 100644 --- a/rclcpp/test/test_service.cpp +++ b/rclcpp/test/test_service.cpp @@ -78,7 +78,8 @@ TEST_F(TestService, construction_and_destruction) { } { - ASSERT_THROW({ + ASSERT_THROW( + { auto service = node->create_service("invalid_service?", callback); }, rclcpp::exceptions::InvalidServiceNameError); } @@ -98,7 +99,8 @@ TEST_F(TestServiceSub, construction_and_destruction) { } { - ASSERT_THROW({ + ASSERT_THROW( + { auto service = node->create_service("invalid_service?", callback); }, rclcpp::exceptions::InvalidServiceNameError); } diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp index 20b0999da1..7f9bed409b 100644 --- a/rclcpp/test/test_subscription.cpp +++ b/rclcpp/test/test_subscription.cpp @@ -129,7 +129,8 @@ TEST_F(TestSubscription, construction_and_destruction) { } { - ASSERT_THROW({ + ASSERT_THROW( + { auto sub = node->create_subscription("invalid_topic?", 10, callback); }, rclcpp::exceptions::InvalidTopicNameError); } @@ -161,7 +162,8 @@ TEST_F(TestSubscriptionSub, construction_and_destruction) { } { - ASSERT_THROW({ + ASSERT_THROW( + { auto sub = node->create_subscription("invalid_topic?", 1, callback); }, rclcpp::exceptions::InvalidTopicNameError); } diff --git a/rclcpp/test/test_time.cpp b/rclcpp/test/test_time.cpp index 6146efbc94..22c9772647 100644 --- a/rclcpp/test/test_time.cpp +++ b/rclcpp/test/test_time.cpp @@ -97,13 +97,15 @@ TEST(TestTime, conversions) { negative_time_msg.sec = -1; negative_time_msg.nanosec = 1; - EXPECT_ANY_THROW({ + EXPECT_ANY_THROW( + { rclcpp::Time negative_time = negative_time_msg; }); EXPECT_ANY_THROW(rclcpp::Time(-1, 1)); - EXPECT_ANY_THROW({ + EXPECT_ANY_THROW( + { rclcpp::Time assignment(1, 2); assignment = negative_time_msg; }); diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index 1d666e57cb..bd8a1868a8 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/rclcpp/test/test_time_source.cpp @@ -161,7 +161,8 @@ void set_use_sim_time_parameter( using namespace std::chrono_literals; EXPECT_TRUE(parameters_client->wait_for_service(2s)); - auto set_parameters_results = parameters_client->set_parameters({ + auto set_parameters_results = parameters_client->set_parameters( + { rclcpp::Parameter("use_sim_time", value) }); for (auto & result : set_parameters_results) { diff --git a/rclcpp/test/test_timer.cpp b/rclcpp/test/test_timer.cpp index 1cdfce957d..2d9022e045 100644 --- a/rclcpp/test/test_timer.cpp +++ b/rclcpp/test/test_timer.cpp @@ -40,17 +40,18 @@ class TestTimer : public ::testing::Test test_node = std::make_shared("test_timer_node"); - timer = test_node->create_wall_timer(100ms, - [this]() -> void - { - this->has_timer_run.store(true); - - if (this->cancel_timer.load()) { - this->timer->cancel(); - } - // prevent any tests running timer from blocking - this->executor->cancel(); + timer = test_node->create_wall_timer( + 100ms, + [this]() -> void + { + this->has_timer_run.store(true); + + if (this->cancel_timer.load()) { + this->timer->cancel(); } + // prevent any tests running timer from blocking + this->executor->cancel(); + } ); executor->add_node(test_node); diff --git a/rclcpp/test/test_utilities.cpp b/rclcpp/test/test_utilities.cpp index 7b84aaf3a6..1f7a03b62b 100644 --- a/rclcpp/test/test_utilities.cpp +++ b/rclcpp/test/test_utilities.cpp @@ -37,7 +37,8 @@ TEST(TestUtilities, remove_ros_arguments) { TEST(TestUtilities, remove_ros_arguments_null) { // In the case of a C executable, we would expect to get // argc=1 and argv = ["process_name"], so this is an invalid input. - ASSERT_THROW({ + ASSERT_THROW( + { rclcpp::remove_ros_arguments(0, nullptr); }, rclcpp::exceptions::RCLErrorBase); } diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 2a491014f1..c588e7125d 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -140,8 +140,7 @@ ClientGoalHandle::invalidate() { std::lock_guard guard(handle_mutex_); status_ = GoalStatus::STATUS_UNKNOWN; - result_promise_.set_exception(std::make_exception_ptr( - exceptions::UnawareGoalHandleError())); + result_promise_.set_exception(std::make_exception_ptr(exceptions::UnawareGoalHandleError())); } template diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index c7237cd3c2..df21d3469a 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -89,7 +89,8 @@ ServerBase::ServerBase( rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); rcl_ret_t ret = rcl_action_server_fini(ptr, rcl_node); (void)ret; - RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), "failed to fini rcl_action_server_t in deleter"); } delete ptr; @@ -254,7 +255,8 @@ ServerBase::execute_goal_request_received() if (nullptr != ptr) { rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr); (void)fail_ret; - RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), "failed to fini rcl_action_goal_handle_t in deleter"); delete ptr; } @@ -329,7 +331,8 @@ ServerBase::execute_cancel_request_received() rclcpp::exceptions::throw_from_rcl_error(ret); } - RCLCPP_SCOPE_EXIT({ + RCLCPP_SCOPE_EXIT( + { ret = rcl_action_cancel_response_fini(&cancel_response); if (RCL_RET_OK != ret) { RCLCPP_ERROR(pimpl_->logger_, "Failed to fini cancel response"); @@ -478,7 +481,8 @@ ServerBase::publish_status() rclcpp::exceptions::throw_from_rcl_error(ret); } - RCLCPP_SCOPE_EXIT({ + RCLCPP_SCOPE_EXIT( + { ret = rcl_action_goal_status_array_fini(&c_status_array); if (RCL_RET_OK != ret) { RCLCPP_ERROR(pimpl_->logger_, "Failed to fini status array message"); diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 1ccc2e745f..82a6dbda60 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -396,7 +396,8 @@ TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result) [&result_callback_received]( const typename ActionGoalHandle::WrappedResult & result) mutable { - if (rclcpp_action::ResultCode::SUCCEEDED == result.code && + if ( + rclcpp_action::ResultCode::SUCCEEDED == result.code && result.result->sequence.size() == 5u) { result_callback_received = true; diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 03159a9f0f..35e2c4e032 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -48,7 +48,8 @@ class TestServer : public ::testing::Test auto request = std::make_shared(); request->goal_id.uuid = uuid; auto future = client->async_send_request(request); - if (rclcpp::executor::FutureReturnCode::SUCCESS != + if ( + rclcpp::executor::FutureReturnCode::SUCCESS != rclcpp::spin_until_future_complete(node, future)) { throw std::runtime_error("send goal future didn't complete succesfully"); @@ -67,7 +68,8 @@ class TestServer : public ::testing::Test auto request = std::make_shared(); request->goal_info.goal_id.uuid = uuid; auto future = cancel_client->async_send_request(request); - if (rclcpp::executor::FutureReturnCode::SUCCESS != + if ( + rclcpp::executor::FutureReturnCode::SUCCESS != rclcpp::spin_until_future_complete(node, future)) { throw std::runtime_error("cancel goal future didn't complete succesfully"); @@ -81,14 +83,15 @@ TEST_F(TestServer, construction_and_destruction) auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); using GoalHandle = rclcpp_action::ServerGoalHandle; - auto as = rclcpp_action::create_server(node, "fibonacci", - [](const GoalUUID &, std::shared_ptr) { - return rclcpp_action::GoalResponse::REJECT; - }, - [](std::shared_ptr) { - return rclcpp_action::CancelResponse::REJECT; - }, - [](std::shared_ptr) {}); + auto as = rclcpp_action::create_server( + node, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); (void)as; } @@ -105,12 +108,13 @@ TEST_F(TestServer, handle_goal_called) }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - [](std::shared_ptr) { - return rclcpp_action::CancelResponse::REJECT; - }, - [](std::shared_ptr) {}); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); (void)as; // Create a client that calls the goal request service @@ -153,12 +157,13 @@ TEST_F(TestServer, handle_accepted_called) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - [](std::shared_ptr) { - return rclcpp_action::CancelResponse::REJECT; - }, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + handle_accepted); (void)as; auto request = send_goal_request(node, uuid); @@ -193,10 +198,11 @@ TEST_F(TestServer, handle_cancel_called) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); (void)as; send_goal_request(node, uuid); @@ -233,10 +239,11 @@ TEST_F(TestServer, handle_cancel_reject) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); (void)as; send_goal_request(node, uuid); @@ -275,10 +282,11 @@ TEST_F(TestServer, handle_cancel_unknown_goal) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); (void)as; send_goal_request(node, uuid); @@ -317,10 +325,11 @@ TEST_F(TestServer, handle_cancel_terminated_goal) handle->succeed(std::make_shared()); }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); (void)as; send_goal_request(node, uuid); @@ -358,10 +367,11 @@ TEST_F(TestServer, publish_status_accepted) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); (void)as; // Subscribe to status messages @@ -420,10 +430,11 @@ TEST_F(TestServer, publish_status_canceling) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); (void)as; // Subscribe to status messages @@ -476,10 +487,11 @@ TEST_F(TestServer, publish_status_canceled) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); (void)as; // Subscribe to status messages @@ -534,10 +546,11 @@ TEST_F(TestServer, publish_status_succeeded) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); (void)as; // Subscribe to status messages @@ -590,10 +603,11 @@ TEST_F(TestServer, publish_status_aborted) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); (void)as; // Subscribe to status messages @@ -646,10 +660,11 @@ TEST_F(TestServer, publish_feedback) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); (void)as; // Subscribe to feedback messages @@ -703,10 +718,11 @@ TEST_F(TestServer, get_result) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); (void)as; send_goal_request(node, uuid); @@ -727,7 +743,8 @@ TEST_F(TestServer, get_result) received_handle->succeed(result); // Wait for the result request to be received - ASSERT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, rclcpp::spin_until_future_complete(node, future)); auto response = future.get(); @@ -759,10 +776,11 @@ TEST_F(TestServer, deferred_execution) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); (void)as; send_goal_request(node, uuid); diff --git a/rclcpp_components/include/rclcpp_components/register_node_macro.hpp b/rclcpp_components/include/rclcpp_components/register_node_macro.hpp index e851bb09db..340d8021bb 100644 --- a/rclcpp_components/include/rclcpp_components/register_node_macro.hpp +++ b/rclcpp_components/include/rclcpp_components/register_node_macro.hpp @@ -31,7 +31,8 @@ * Note: NodeClass does not need to inherit from `rclcpp::Node`, but it is the easiest way. */ #define RCLCPP_COMPONENTS_REGISTER_NODE(NodeClass) \ - CLASS_LOADER_REGISTER_CLASS(rclcpp_components::NodeFactoryTemplate, \ + CLASS_LOADER_REGISTER_CLASS( \ + rclcpp_components::NodeFactoryTemplate, \ rclcpp_components::NodeFactory) #endif // RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__ diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index b7e8746db0..43d18fa084 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -33,12 +33,15 @@ ComponentManager::ComponentManager( : Node("ComponentManager"), executor_(executor) { - loadNode_srv_ = create_service("~/_container/load_node", - std::bind(&ComponentManager::OnLoadNode, this, _1, _2, _3)); - unloadNode_srv_ = create_service("~/_container/unload_node", - std::bind(&ComponentManager::OnUnloadNode, this, _1, _2, _3)); - listNodes_srv_ = create_service("~/_container/list_nodes", - std::bind(&ComponentManager::OnListNodes, this, _1, _2, _3)); + loadNode_srv_ = create_service( + "~/_container/load_node", + std::bind(&ComponentManager::OnLoadNode, this, _1, _2, _3)); + unloadNode_srv_ = create_service( + "~/_container/unload_node", + std::bind(&ComponentManager::OnUnloadNode, this, _1, _2, _3)); + listNodes_srv_ = create_service( + "~/_container/list_nodes", + std::bind(&ComponentManager::OnListNodes, this, _1, _2, _3)); } ComponentManager::~ComponentManager() @@ -58,7 +61,8 @@ ComponentManager::get_component_resources(const std::string & package_name) cons { std::string content; std::string base_path; - if (!ament_index_cpp::get_resource( + if ( + !ament_index_cpp::get_resource( "rclcpp_components", package_name, content, &base_path)) { throw ComponentManagerException("Could not find requested resource in ament index"); diff --git a/rclcpp_components/test/test_component_manager.cpp b/rclcpp_components/test/test_component_manager.cpp index a690111030..a86bbe54ff 100644 --- a/rclcpp_components/test/test_component_manager.cpp +++ b/rclcpp_components/test/test_component_manager.cpp @@ -34,7 +34,8 @@ TEST_F(TestComponentManager, get_component_resources_invalid) auto exec = std::make_shared(); auto manager = std::make_shared(exec); - EXPECT_THROW(manager->get_component_resources("invalid_package"), + EXPECT_THROW( + manager->get_component_resources("invalid_package"), rclcpp_components::ComponentManagerException); } @@ -79,7 +80,8 @@ TEST_F(TestComponentManager, create_component_factory_invalid) auto manager = std::make_shared(exec); // Test invalid library - EXPECT_THROW(manager->create_component_factory({"foo_class", "invalid_library.so"}), + EXPECT_THROW( + manager->create_component_factory({"foo_class", "invalid_library.so"}), rclcpp_components::ComponentManagerException); // Test valid library with invalid class diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index e9429a10cb..41d925a75c 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -305,9 +305,7 @@ LifecycleNode::set_parameter_if_not_set( { rclcpp::Parameter parameter; if (!this->get_parameter(name, parameter)) { - this->set_parameters({ - rclcpp::Parameter(name, value), - }); + this->set_parameters({rclcpp::Parameter(name, value), }); } } @@ -376,9 +374,7 @@ LifecycleNode::get_parameter_or_set( { bool got_parameter = get_parameter(name, value); if (!got_parameter) { - this->set_parameters({ - rclcpp::Parameter(name, alternative_value), - }); + this->set_parameters({rclcpp::Parameter(name, alternative_value), }); value = alternative_value; } } diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index 9f58d31fa4..43d7bf622f 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -83,7 +83,8 @@ class LifecyclePublisher : public LifecyclePublisherInterface, publish(std::unique_ptr msg) { if (!enabled_) { - RCLCPP_WARN(logger_, + RCLCPP_WARN( + logger_, "Trying to publish message on the topic '%s', but the publisher is not activated", this->get_topic_name()); @@ -102,7 +103,8 @@ class LifecyclePublisher : public LifecyclePublisherInterface, publish(const MessageT & msg) { if (!enabled_) { - RCLCPP_WARN(logger_, + RCLCPP_WARN( + logger_, "Trying to publish message on the topic '%s', but the publisher is not activated", this->get_topic_name()); @@ -130,7 +132,8 @@ class LifecyclePublisher : public LifecyclePublisherInterface, publish(const std::shared_ptr & msg) { if (!enabled_) { - RCLCPP_WARN(logger_, + RCLCPP_WARN( + logger_, "Trying to publish message on the topic '%s', but the publisher is not activated", this->get_topic_name()); diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 736cab953c..edecd8d31f 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -114,15 +114,23 @@ LifecycleNode::LifecycleNode( { impl_->init(); - register_on_configure(std::bind(&LifecycleNodeInterface::on_configure, this, - std::placeholders::_1)); + register_on_configure( + std::bind( + &LifecycleNodeInterface::on_configure, this, + std::placeholders::_1)); register_on_cleanup(std::bind(&LifecycleNodeInterface::on_cleanup, this, std::placeholders::_1)); - register_on_shutdown(std::bind(&LifecycleNodeInterface::on_shutdown, this, - std::placeholders::_1)); - register_on_activate(std::bind(&LifecycleNodeInterface::on_activate, this, - std::placeholders::_1)); - register_on_deactivate(std::bind(&LifecycleNodeInterface::on_deactivate, this, - std::placeholders::_1)); + register_on_shutdown( + std::bind( + &LifecycleNodeInterface::on_shutdown, this, + std::placeholders::_1)); + register_on_activate( + std::bind( + &LifecycleNodeInterface::on_activate, this, + std::placeholders::_1)); + register_on_deactivate( + std::bind( + &LifecycleNodeInterface::on_deactivate, this, + std::placeholders::_1)); register_on_error(std::bind(&LifecycleNodeInterface::on_error, this, std::placeholders::_1)); } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index c55a0b0467..b7fa373917 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -105,8 +105,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } { // change_state - auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_change_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_change_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); rclcpp::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); @@ -120,8 +121,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } { // get_state - auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_get_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); rclcpp::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); @@ -135,8 +137,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } { // get_available_states - auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_available_states, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_get_available_states, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); rclcpp::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); @@ -150,8 +153,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } { // get_available_transitions - auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_available_transitions, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_get_available_transitions, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); rclcpp::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); @@ -166,8 +170,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } { // get_transition_graph - auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_transition_graph, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_get_transition_graph, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); rclcpp::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); @@ -355,7 +360,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl change_state(std::uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) { if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Unable to change state for state machine for %s: %s", + RCUTILS_LOG_ERROR( + "Unable to change state for state machine for %s: %s", node_base_interface_->get_name(), rcl_get_error_string().str); return RCL_RET_ERROR; } @@ -364,10 +370,12 @@ class LifecycleNode::LifecycleNodeInterfaceImpl // keep the initial state to pass to a transition callback State initial_state(state_machine_.current_state); - if (rcl_lifecycle_trigger_transition_by_id( + if ( + rcl_lifecycle_trigger_transition_by_id( &state_machine_, transition_id, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Unable to start transition %u from current state %s: %s", + RCUTILS_LOG_ERROR( + "Unable to start transition %u from current state %s: %s", transition_id, state_machine_.current_state->label, rcl_get_error_string().str); return RCL_RET_ERROR; } @@ -386,10 +394,12 @@ class LifecycleNode::LifecycleNodeInterfaceImpl cb_return_code = execute_callback(state_machine_.current_state->id, initial_state); auto transition_label = get_label_for_return_code(cb_return_code); - if (rcl_lifecycle_trigger_transition_by_label( + if ( + rcl_lifecycle_trigger_transition_by_label( &state_machine_, transition_label, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Failed to finish transition %u. Current state is now: %s", + RCUTILS_LOG_ERROR( + "Failed to finish transition %u. Current state is now: %s", transition_id, state_machine_.current_state->label); return RCL_RET_ERROR; } @@ -401,7 +411,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state); auto error_cb_label = get_label_for_return_code(error_cb_code); - if (rcl_lifecycle_trigger_transition_by_label( + if ( + rcl_lifecycle_trigger_transition_by_label( &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) { RCUTILS_LOG_ERROR("Failed to call cleanup on error state"); diff --git a/rclcpp_lifecycle/test/test_callback_exceptions.cpp b/rclcpp_lifecycle/test/test_callback_exceptions.cpp index 30c10a858a..7c4117ae7b 100644 --- a/rclcpp_lifecycle/test/test_callback_exceptions.cpp +++ b/rclcpp_lifecycle/test/test_callback_exceptions.cpp @@ -65,7 +65,8 @@ TEST_F(TestCallbackExceptions, positive_on_error) { auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten EXPECT_EQ(2u, test_node->number_of_callbacks); @@ -110,7 +111,8 @@ TEST_F(TestCallbackExceptions, negative_on_error) { auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten EXPECT_EQ(2u, test_node->number_of_callbacks); diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 1ce941cad2..e8066370e0 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -143,15 +143,20 @@ TEST_F(TestDefaultStateMachine, trigger_transition) { auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + ASSERT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + ASSERT_EQ( + State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + ASSERT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); - ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + ASSERT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); - ASSERT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + ASSERT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); } @@ -187,15 +192,20 @@ TEST_F(TestDefaultStateMachine, good_mood) { auto test_node = std::make_shared>("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); - EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); // check if all callbacks were successfully overwritten @@ -206,7 +216,8 @@ TEST_F(TestDefaultStateMachine, bad_mood) { auto test_node = std::make_shared>("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten diff --git a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp index aea561655a..3c146e4207 100644 --- a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp +++ b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp @@ -137,27 +137,42 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode TEST_F(TestRegisterCustomCallbacks, custom_callbacks) { auto test_node = std::make_shared("testnode"); - test_node->register_on_configure(std::bind(&CustomLifecycleNode::on_custom_configure, - test_node.get(), std::placeholders::_1)); - test_node->register_on_cleanup(std::bind(&CustomLifecycleNode::on_custom_cleanup, - test_node.get(), std::placeholders::_1)); - test_node->register_on_shutdown(std::bind(&CustomLifecycleNode::on_custom_shutdown, - test_node.get(), std::placeholders::_1)); - test_node->register_on_activate(std::bind(&CustomLifecycleNode::on_custom_activate, - test_node.get(), std::placeholders::_1)); - test_node->register_on_deactivate(std::bind(&CustomLifecycleNode::on_custom_deactivate, - test_node.get(), std::placeholders::_1)); + test_node->register_on_configure( + std::bind( + &CustomLifecycleNode::on_custom_configure, + test_node.get(), std::placeholders::_1)); + test_node->register_on_cleanup( + std::bind( + &CustomLifecycleNode::on_custom_cleanup, + test_node.get(), std::placeholders::_1)); + test_node->register_on_shutdown( + std::bind( + &CustomLifecycleNode::on_custom_shutdown, + test_node.get(), std::placeholders::_1)); + test_node->register_on_activate( + std::bind( + &CustomLifecycleNode::on_custom_activate, + test_node.get(), std::placeholders::_1)); + test_node->register_on_deactivate( + std::bind( + &CustomLifecycleNode::on_custom_deactivate, + test_node.get(), std::placeholders::_1)); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); - EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); // check if all callbacks were successfully overwritten diff --git a/rclcpp_lifecycle/test/test_state_machine_info.cpp b/rclcpp_lifecycle/test/test_state_machine_info.cpp index b14441e8cf..dfa7bdf1ca 100644 --- a/rclcpp_lifecycle/test/test_state_machine_info.cpp +++ b/rclcpp_lifecycle/test/test_state_machine_info.cpp @@ -60,12 +60,14 @@ TEST_F(TestStateMachineInfo, available_transitions) { for (rclcpp_lifecycle::Transition & transition : available_transitions) { EXPECT_FALSE(transition.label().empty()); - EXPECT_TRUE(transition.start_state().id() <= 4 || + EXPECT_TRUE( + transition.start_state().id() <= 4 || (transition.start_state().id() >= 10 && (transition.start_state().id() <= 15))); EXPECT_FALSE(transition.start_state().label().empty()); - EXPECT_TRUE(transition.goal_state().id() <= 4 || + EXPECT_TRUE( + transition.goal_state().id() <= 4 || (transition.goal_state().id() >= 10 && (transition.goal_state().id() <= 15))); EXPECT_FALSE(transition.goal_state().label().empty());