Adding more simple action server, nav2_utils, and lifecycle bringup CLI tests

nav2_util/include/nav2_util/simple_action_server.hpp

@@ -322,6 +322,7 @@ class SimpleActionServer
   {
     if (!is_active(current_handle_)) {
       error_msg("Trying to publish feedback when the current goal handle is not active");
+      return;
     }
 
     current_handle_->publish_feedback(feedback);

nav2_util/test/CMakeLists.txt

@@ -24,3 +24,7 @@ target_link_libraries(test_actions ${library_name})
 ament_add_gtest(test_lifecycle_node test_lifecycle_node.cpp)
 ament_target_dependencies(test_lifecycle_node rclcpp_lifecycle)
 target_link_libraries(test_lifecycle_node ${library_name})
+
+ament_add_gtest(test_lifecycle_cli_node test_lifecycle_cli_node.cpp)
+ament_target_dependencies(test_lifecycle_cli_node rclcpp_lifecycle)
+target_link_libraries(test_lifecycle_cli_node ${library_name})

nav2_util/test/test_actions.cpp

@@ -74,6 +74,13 @@ class FibonacciServerNode : public rclcpp::Node
 
   void on_term()
   {
+    // when nothing's running make sure everything's dead.
+    const std::shared_ptr<const Fibonacci::Goal> a = action_server_->accept_pending_goal();
+    const std::shared_ptr<const Fibonacci::Goal> b = action_server_->get_current_goal();
+    assert(a == b);
+    assert(action_server_->is_cancel_requested() == false);
+    auto feedback = std::make_shared<Fibonacci::Feedback>();
+    action_server_->publish_feedback(feedback);
     action_server_.reset();
   }
 
@@ -489,6 +496,24 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded)
   SUCCEED();
 }
 
+TEST_F(ActionTest, test_handle_goal_deactivated)
+{
+  node_->deactivate_server();
+  auto goal = Fibonacci::Goal();
+  goal.order = 12;
+
+  // Send the goal
+  auto future_goal_handle = node_->action_client_->async_send_goal(goal);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS);
+
+  node_->activate_server();
+
+  SUCCEED();
+}
+
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);

nav2_util/test/test_lifecycle_cli_node.cpp

@@ -0,0 +1,100 @@
+// Copyright (c) 2020 Samsung Research
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_
+#define NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_
+
+#include <cstdlib>
+#include <memory>
+#include "gtest/gtest.h"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/lifecycle_utils.hpp"
+#include "nav2_util/node_thread.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class DummyNode : public nav2_util::LifecycleNode
+{
+public:
+  DummyNode()
+  : nav2_util::LifecycleNode("nav2_test_cli", "")
+  {
+    activated = false;
+  }
+
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/)
+  {
+    activated = true;
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  bool activated;
+};
+
+class Handle
+{
+public:
+  Handle()
+  {
+    node = std::make_shared<DummyNode>();
+    thread = std::make_shared<nav2_util::NodeThread>(node->get_node_base_interface());
+  }
+  ~Handle()
+  {
+    thread.reset();
+    node.reset();
+  }
+
+  std::shared_ptr<nav2_util::NodeThread> thread;
+  std::shared_ptr<DummyNode> node;
+};
+
+class RclCppFixture
+{
+public:
+  RclCppFixture()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  ~RclCppFixture()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+RclCppFixture g_rclcppfixture;
+
+TEST(LifeycleCLI, fails_no_node_name)
+{
+  Handle handle;
+  auto rc = system("ros2 run nav2_util lifecycle_bringup");
+  (void)rc;
+  sleep(1);
+  // check node didn't mode
+  EXPECT_EQ(handle.node->activated, false);
+  SUCCEED();
+}
+
+TEST(LifeycleCLI, succeeds_node_name)
+{
+  Handle handle;
+  auto rc = system("ros2 run nav2_util lifecycle_bringup nav2_test_cli");
+  sleep(3);
+  // check node moved
+  (void)rc;
+  EXPECT_EQ(handle.node->activated, true);
+  SUCCEED();
+}
+
+#endif  // NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_

nav2_util/test/test_node_utils.cpp

@@ -18,6 +18,7 @@
 using nav2_util::sanitize_node_name;
 using nav2_util::generate_internal_node_name;
 using nav2_util::generate_internal_node;
+using nav2_util::add_namespaces;
 using nav2_util::time_to_string;
 
 TEST(SanitizeNodeName, SanitizeNodeName)
@@ -47,3 +48,9 @@ TEST(GenerateInternalNodeName, GenerateNodeName)
   ASSERT_EQ(defaultName[0], '_');
   ASSERT_EQ(defaultName.length(), 9u);
 }
+
+TEST(AddNamespaces, AddNamespaceSlash)
+{
+  ASSERT_EQ(add_namespaces("hi", "bye"), "hi/bye");
+  ASSERT_EQ(add_namespaces("hi/", "bye"), "/hi/bye");
+}