From c8941bb72bef2309dee04a63c55f5b4437329632 Mon Sep 17 00:00:00 2001 From: Christian Henkel <6976069+ct2034@users.noreply.github.com> Date: Wed, 7 Jun 2023 12:09:50 +0200 Subject: [PATCH] Self test publishes the service under the node name, again (#269) * splitting out examples into two, to test how this works * adding argument for node name * removing experiments with second example Signed-off-by: Christian Henkel --- .github/workflows/lint.yaml | 2 +- self_test/CMakeLists.txt | 2 +- .../{src => example}/selftest_example.cpp | 5 +- self_test/include/self_test/test_runner.hpp | 41 +++---- self_test/src/run_selftest.cpp | 105 ++++++++++-------- self_test/test/error_selftest.cpp | 3 +- self_test/test/exception_selftest.cpp | 3 +- self_test/test/no_id_selftest.cpp | 3 +- self_test/test/nominal_selftest.cpp | 3 +- 9 files changed, 94 insertions(+), 73 deletions(-) rename self_test/{src => example}/selftest_example.cpp (97%) diff --git a/.github/workflows/lint.yaml b/.github/workflows/lint.yaml index a2109cc20..010a90157 100644 --- a/.github/workflows/lint.yaml +++ b/.github/workflows/lint.yaml @@ -28,7 +28,7 @@ jobs: steps: - uses: actions/checkout@v1 - uses: ros-tooling/setup-ros@master - - run: sudo pip install pydocstyle==6.1.1 # downgrade to fix https://github.com/ament/ament_lint/pull/428 + - run: sudo pip install pydocstyle==6.1.1 # downgrade to fix https://github.com/ament/ament_lint/pull/428 - uses: ros-tooling/action-ros-lint@master with: linter: ${{ matrix.linter }} diff --git a/self_test/CMakeLists.txt b/self_test/CMakeLists.txt index a39354e74..0acbaa640 100644 --- a/self_test/CMakeLists.txt +++ b/self_test/CMakeLists.txt @@ -47,7 +47,7 @@ ament_target_dependencies( "rcutils" ) -add_executable(selftest_example src/selftest_example.cpp) +add_executable(selftest_example example/selftest_example.cpp) target_include_directories(selftest_example PUBLIC $ diff --git a/self_test/src/selftest_example.cpp b/self_test/example/selftest_example.cpp similarity index 97% rename from self_test/src/selftest_example.cpp rename to self_test/example/selftest_example.cpp index dd5b9f03c..cc5dbd129 100644 --- a/self_test/src/selftest_example.cpp +++ b/self_test/example/selftest_example.cpp @@ -46,7 +46,7 @@ class MyNode : public rclcpp::Node public: MyNode() - : rclcpp::Node("self_test_node"), + : rclcpp::Node("self_test_example_node"), self_test_( get_node_base_interface(), get_node_services_interface(), get_node_logging_interface()) { @@ -123,9 +123,6 @@ class MyNode : public rclcpp::Node status.level = 0; - // Exceptions of time runtime_error will actually propagate messages - throw std::runtime_error("we did something that threw an exception"); - // Here's where we would report success if we'd made it past status.summary( diagnostic_msgs::msg::DiagnosticStatus::OK, diff --git a/self_test/include/self_test/test_runner.hpp b/self_test/include/self_test/test_runner.hpp index 78d669180..72793c61d 100644 --- a/self_test/include/self_test/test_runner.hpp +++ b/self_test/include/self_test/test_runner.hpp @@ -69,7 +69,8 @@ class TestRunner : public diagnostic_updater::DiagnosticTaskVector TestRunner( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr base_interface, - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr service_interface, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr + service_interface, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) : base_interface_(base_interface), service_interface_(service_interface), @@ -79,10 +80,11 @@ class TestRunner : public diagnostic_updater::DiagnosticTaskVector RCLCPP_DEBUG(logger_, "Advertising self_test"); auto serviceCB = - [this](std::shared_ptr request, - std::shared_ptr response) -> bool - { - (void) request; + [this]( + std::shared_ptr request, + std::shared_ptr response) + -> bool { + (void)request; bool retval = false; bool ignore_set_id_warn = false; @@ -95,7 +97,8 @@ class TestRunner : public diagnostic_updater::DiagnosticTaskVector std::vector status_vec; const std::vector & tasks = getTasks(); - for (std::vector::const_iterator iter = tasks.begin(); + for (std::vector::const_iterator iter = + tasks.begin(); iter != tasks.end(); iter++) { diagnostic_updater::DiagnosticStatusWrapper status; @@ -116,8 +119,10 @@ class TestRunner : public diagnostic_updater::DiagnosticTaskVector if (verbose) { RCLCPP_WARN( logger_, - "Non-zero self-test test status. Name: %s Status %i: Message: %s", - status.name.c_str(), status.level, status.message.c_str()); + "Non-zero self-test test status. Name: %s Status %i: " + "Message: %s", + status.name.c_str(), status.level, + status.message.c_str()); } } status_vec.push_back(status); @@ -129,10 +134,9 @@ class TestRunner : public diagnostic_updater::DiagnosticTaskVector response->id = id_; response->passed = true; - for (std::vector::iterator status_iter = - status_vec.begin(); - status_iter != status_vec.end(); - status_iter++) + for (std::vector::iterator + status_iter = status_vec.begin(); + status_iter != status_vec.end(); status_iter++) { if (status_iter->level >= 2) { response->passed = false; @@ -142,7 +146,8 @@ class TestRunner : public diagnostic_updater::DiagnosticTaskVector if (response->passed && id_ == unspecified_id) { RCLCPP_WARN( logger_, - "Self-test passed, but setID was not called. This is a bug in the driver."); + "Self-test passed, but setID was not called. This is a " + "bug in the driver."); } response->status = status_vec; @@ -155,15 +160,13 @@ class TestRunner : public diagnostic_updater::DiagnosticTaskVector }; service_server_ = rclcpp::create_service( - base_interface_, service_interface_, "self_test", serviceCB, rmw_qos_profile_default, - nullptr); + base_interface_, service_interface_, + std::string(base_interface_->get_name()) + std::string("/self_test"), + serviceCB, rmw_qos_profile_default, nullptr); verbose = true; } - void setID(std::string id) - { - id_ = id; - } + void setID(std::string id) {id_ = id;} }; } // namespace self_test #endif // SELF_TEST__TEST_RUNNER_HPP_ diff --git a/self_test/src/run_selftest.cpp b/self_test/src/run_selftest.cpp index 74636623c..32c6ee876 100644 --- a/self_test/src/run_selftest.cpp +++ b/self_test/src/run_selftest.cpp @@ -1,55 +1,57 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ #include #include #include #include "diagnostic_msgs/srv/self_test.hpp" - #include "rclcpp/rclcpp.hpp" class ClientNode : public rclcpp::Node { public: - ClientNode() - : Node("self_test_client") + explicit ClientNode(std::string _node_name) + : Node("self_test_client"), node_name_to_test(_node_name) { - client_ = create_client("self_test"); + client_ = create_client( + node_name_to_test + + "/self_test"); } - rclcpp::Client::SharedFuture queue_async_request() + rclcpp::Client::SharedFuture + queue_async_request() { using namespace std::chrono_literals; using ServiceResponseFuture = @@ -57,10 +59,14 @@ class ClientNode : public rclcpp::Node while (!client_->wait_for_service(1s)) { if (!rclcpp::ok()) { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + RCLCPP_ERROR( + this->get_logger(), + "Interrupted while waiting for the service. Exiting."); return ServiceResponseFuture(); } - RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + RCLCPP_INFO( + this->get_logger(), + "service not available, waiting again..."); } auto request = std::make_shared(); @@ -69,12 +75,15 @@ class ClientNode : public rclcpp::Node RCLCPP_INFO( this->get_logger(), "Self test %s for device with id: [%s]", - (result_out->passed ? "PASSED" : "FAILED"), result_out->id.c_str()); + (result_out->passed ? "PASSED" : "FAILED"), + result_out->id.c_str()); // for (size_t i = 0; i < result_out->status.size(); i++) { auto counter = 1lu; for (const auto & status : result_out->status) { - RCLCPP_INFO(this->get_logger(), "%zu) %s", counter++, status.name.c_str()); + RCLCPP_INFO( + this->get_logger(), "%zu) %s", counter++, + status.name.c_str()); if (status.level == 0) { RCLCPP_INFO(this->get_logger(), "\t%s", status.message.c_str()); } else if (status.level == 1) { @@ -85,8 +94,7 @@ class ClientNode : public rclcpp::Node for (const auto & kv : status.values) { RCLCPP_INFO( - this->get_logger(), "\t[%s] %s", - kv.key.c_str(), + this->get_logger(), "\t[%s] %s", kv.key.c_str(), kv.value.c_str()); } } @@ -100,12 +108,21 @@ class ClientNode : public rclcpp::Node private: rclcpp::Client::SharedPtr client_; + std::string node_name_to_test; }; int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + if (argc != 2) { + RCLCPP_ERROR( + rclcpp::get_logger("run_selftest"), + "usage: run_selftest NODE_NAME"); + return 1; + } + std::string node_name = argv[1]; + + auto node = std::make_shared(node_name); auto async_srv_request = node->queue_async_request(); if (!async_srv_request.valid()) { rclcpp::shutdown(); diff --git a/self_test/test/error_selftest.cpp b/self_test/test/error_selftest.cpp index 53a761876..878c882df 100644 --- a/self_test/test/error_selftest.cpp +++ b/self_test/test/error_selftest.cpp @@ -70,7 +70,8 @@ class ErrorSelftestNode : public SelftestNode using Fixture = SelftestFixture; TEST_F(Fixture, run_self_test) { - auto client = node_->create_client("self_test"); + auto client = node_->create_client( + "error_selftest_node/self_test"); using namespace std::chrono_literals; if (!client->wait_for_service(5s)) { diff --git a/self_test/test/exception_selftest.cpp b/self_test/test/exception_selftest.cpp index 2c404c272..119b5978b 100644 --- a/self_test/test/exception_selftest.cpp +++ b/self_test/test/exception_selftest.cpp @@ -78,7 +78,8 @@ class ExceptionSelftestNode : public SelftestNode using Fixture = SelftestFixture; TEST_F(Fixture, run_self_test) { - auto client = node_->create_client("self_test"); + auto client = node_->create_client( + "exception_selftest_node/self_test"); using namespace std::chrono_literals; if (!client->wait_for_service(5s)) { diff --git a/self_test/test/no_id_selftest.cpp b/self_test/test/no_id_selftest.cpp index d468c30e6..b19090a15 100644 --- a/self_test/test/no_id_selftest.cpp +++ b/self_test/test/no_id_selftest.cpp @@ -67,7 +67,8 @@ class NoIDSelftestNode : public SelftestNode using Fixture = SelftestFixture; TEST_F(Fixture, run_self_test) { - auto client = node_->create_client("self_test"); + auto client = + node_->create_client("no_id_selftest_node/self_test"); using namespace std::chrono_literals; if (!client->wait_for_service(5s)) { diff --git a/self_test/test/nominal_selftest.cpp b/self_test/test/nominal_selftest.cpp index c1efce770..d38503cef 100644 --- a/self_test/test/nominal_selftest.cpp +++ b/self_test/test/nominal_selftest.cpp @@ -46,7 +46,8 @@ class NominalSelftestNode : public SelftestNode using Fixture = SelftestFixture; TEST_F(Fixture, run_self_test) { - auto client = node_->create_client("self_test"); + auto client = node_->create_client( + "nominal_selftest_node/self_test"); using namespace std::chrono_literals; if (!client->wait_for_service(5s)) {