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 <[email protected]>
ros · Jun 7, 2023 · c8941bb · c8941bb
1 parent be6fec7
commit c8941bb
Show file tree

Hide file tree

Showing 9 changed files with 94 additions and 73 deletions.
diff --git 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
@@ -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
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

diff --git a/self_test/src/selftest_example.cpp → self_test/example/selftest_example.cpp b/self_test/src/selftest_example.cpp → 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
@@ -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<diagnostic_msgs::srv::SelfTest::Request> request,
-        std::shared_ptr<diagnostic_msgs::srv::SelfTest::Response> response) -> bool
-      {
-        (void) request;
+      [this](
+      std::shared_ptr<diagnostic_msgs::srv::SelfTest::Request> request,
+      std::shared_ptr<diagnostic_msgs::srv::SelfTest::Response> 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<diagnostic_msgs::msg::DiagnosticStatus> status_vec;
 
           const std::vector<DiagnosticTaskInternal> & tasks = getTasks();
-          for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
+          for (std::vector<DiagnosticTaskInternal>::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<diagnostic_msgs::msg::DiagnosticStatus>::iterator status_iter =
-            status_vec.begin();
-            status_iter != status_vec.end();
-            status_iter++)
+          for (std::vector<diagnostic_msgs::msg::DiagnosticStatus>::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<diagnostic_msgs::srv::SelfTest>(
-      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
@@ -1,66 +1,72 @@
 /*********************************************************************
-* 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 <chrono>
 #include <memory>
 #include <string>
 
 #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<diagnostic_msgs::srv::SelfTest>("self_test");
+    client_ = create_client<diagnostic_msgs::srv::SelfTest>(
+      node_name_to_test +
+      "/self_test");
   }
 
-  rclcpp::Client<diagnostic_msgs::srv::SelfTest>::SharedFuture queue_async_request()
+  rclcpp::Client<diagnostic_msgs::srv::SelfTest>::SharedFuture
+  queue_async_request()
   {
     using namespace std::chrono_literals;
     using ServiceResponseFuture =
       rclcpp::Client<diagnostic_msgs::srv::SelfTest>::SharedFuture;
 
     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<diagnostic_msgs::srv::SelfTest::Request>();
 
@@ -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<diagnostic_msgs::srv::SelfTest>::SharedPtr client_;
+  std::string node_name_to_test;
 };
 
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
-  auto node = std::make_shared<ClientNode>();
+  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<ClientNode>(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
@@ -70,7 +70,8 @@ class ErrorSelftestNode : public SelftestNode
 using Fixture = SelftestFixture<ErrorSelftestNode>;
 TEST_F(Fixture, run_self_test)
 {
-  auto client = node_->create_client<diagnostic_msgs::srv::SelfTest>("self_test");
+  auto client = node_->create_client<diagnostic_msgs::srv::SelfTest>(
+    "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
@@ -78,7 +78,8 @@ class ExceptionSelftestNode : public SelftestNode
 using Fixture = SelftestFixture<ExceptionSelftestNode>;
 TEST_F(Fixture, run_self_test)
 {
-  auto client = node_->create_client<diagnostic_msgs::srv::SelfTest>("self_test");
+  auto client = node_->create_client<diagnostic_msgs::srv::SelfTest>(
+    "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
@@ -67,7 +67,8 @@ class NoIDSelftestNode : public SelftestNode
 using Fixture = SelftestFixture<NoIDSelftestNode>;
 TEST_F(Fixture, run_self_test)
 {
-  auto client = node_->create_client<diagnostic_msgs::srv::SelfTest>("self_test");
+  auto client =
+    node_->create_client<diagnostic_msgs::srv::SelfTest>("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
@@ -46,7 +46,8 @@ class NominalSelftestNode : public SelftestNode
 using Fixture = SelftestFixture<NominalSelftestNode>;
 TEST_F(Fixture, run_self_test)
 {
-  auto client = node_->create_client<diagnostic_msgs::srv::SelfTest>("self_test");
+  auto client = node_->create_client<diagnostic_msgs::srv::SelfTest>(
+    "nominal_selftest_node/self_test");
 
   using namespace std::chrono_literals;
   if (!client->wait_for_service(5s)) {