Add test for test_msgs/NestedMessage.action (#330) (#335)

* Add test for test_msgs/NestedMessage.action

Signed-off-by: Shane Loretz <[email protected]>

* Fixes for cpplint and uncrustify

Signed-off-by: Shane Loretz <[email protected]>

test_communication/test/test_action_client.cpp

@@ -14,6 +14,7 @@
 
 #include <chrono>
 #include <functional>
+#include <random>
 #include <string>
 #include <vector>
 
@@ -23,6 +24,7 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 
 #include "test_msgs/action/fibonacci.hpp"
+#include "test_msgs/action/nested_message.hpp"
 
 using namespace std::chrono_literals;
 
@@ -170,6 +172,44 @@ generate_fibonacci_goal_tests()
   return result;
 }
 
+std::vector<ActionClientTest<test_msgs::action::NestedMessage>>
+generate_nested_message_goal_tests()
+{
+  std::vector<ActionClientTest<test_msgs::action::NestedMessage>> result;
+
+  std::default_random_engine generator;
+  std::uniform_int_distribution<int> distribution(1, 12345);
+  const int32_t initial_value = distribution(generator);
+  const int32_t expected_feedback_value = 2 * initial_value;
+  const int32_t expected_result_value = 4 * initial_value;
+
+  {
+    ActionClientTest<test_msgs::action::NestedMessage> test;
+    test.goal.nested_field_no_pkg.duration_value.sec = initial_value;
+    test.result_is_valid =
+      [initial_value, expected_result_value](auto result) -> bool {
+        if (result->nested_field.int32_value != expected_result_value) {
+          fprintf(stderr, "expected result %d but got %d for initial value %d\n",
+            expected_result_value, result->nested_field.int32_value, initial_value);
+          return false;
+        }
+        return true;
+      };
+    test.feedback_is_valid =
+      [initial_value, expected_feedback_value](auto feedback) -> bool {
+        if (feedback->nested_different_pkg.sec != expected_feedback_value) {
+          fprintf(stderr, "expected feedback %d but got %d for initial value %d\n",
+            expected_feedback_value, feedback->nested_different_pkg.sec, initial_value);
+          return false;
+        }
+        return true;
+      };
+    result.push_back(test);
+  }
+
+  return result;
+}
+
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
@@ -185,6 +225,9 @@ int main(int argc, char ** argv)
   int rc;
   if (action == "Fibonacci") {
     rc = send_goals<test_msgs::action::Fibonacci>(node, action, generate_fibonacci_goal_tests());
+  } else if (action == "NestedMessage") {
+    rc = send_goals<test_msgs::action::NestedMessage>(
+      node, action, generate_nested_message_goal_tests());
   } else {
     fprintf(stderr, "Unknown action type '%s'\n", action.c_str());
     return 1;

test_communication/test/test_action_server.cpp

@@ -23,6 +23,7 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 
 #include "test_msgs/action/fibonacci.hpp"
+#include "test_msgs/action/nested_message.hpp"
 
 template<typename ActionT>
 struct ExpectedGoalRequest
@@ -165,6 +166,76 @@ generate_expected_fibonacci_goals(rclcpp::Logger logger)
   return result;
 }
 
+std::vector<ExpectedGoalRequest<test_msgs::action::NestedMessage>>
+generate_expected_nested_message_goals(rclcpp::Logger logger)
+{
+  std::vector<ExpectedGoalRequest<test_msgs::action::NestedMessage>> result;
+
+  {
+    ExpectedGoalRequest<test_msgs::action::NestedMessage> expected_goal;
+
+    expected_goal.goal_is_expected =
+      [](auto goal) {
+        if (goal && goal->nested_field_no_pkg.duration_value.sec > 0) {
+          return true;
+        }
+        return false;
+      };
+
+    expected_goal.goal_is_valid =
+      [](auto) {
+        return true;
+      };
+
+    expected_goal.execute_goal =
+      [logger](auto goal_handle) {
+        const auto goal = goal_handle->get_goal();
+        rclcpp::Rate loop_rate(10);
+        const int32_t initial_value = goal->nested_field_no_pkg.duration_value.sec;
+        const int32_t feedback_value = 2 * initial_value;
+        const int32_t result_value = 4 * initial_value;
+
+        auto feedback = std::make_shared<test_msgs::action::NestedMessage::Feedback>();
+
+        auto result = std::make_shared<test_msgs::action::NestedMessage::Result>();
+
+        if (initial_value <= 0) {
+          RCLCPP_ERROR(logger, "expected a goal > 0, got %d", initial_value);
+          return;
+        }
+        const size_t num_feedback = 10;
+
+        for (size_t i = 1; i < num_feedback; ++i) {
+          if (!rclcpp::ok()) {
+            return;
+          }
+          // Check if the goal was canceled.
+          if (goal_handle->is_canceling()) {
+            result->nested_field.int32_value = result_value;
+            goal_handle->set_canceled(result);
+            RCLCPP_INFO(logger, "goal was canceled");
+            return;
+          }
+          // Update the feedback;
+          feedback->nested_different_pkg.sec = feedback_value;
+          // Publish the current state as feedback.
+          goal_handle->publish_feedback(feedback);
+          RCLCPP_INFO(logger, "publishing feedback for goal");
+
+          loop_rate.sleep();
+        }
+
+        result->nested_field.int32_value = result_value;
+        goal_handle->set_succeeded(result);
+        RCLCPP_INFO(logger, "goal succeeded");
+      };
+
+    result.push_back(expected_goal);
+  }
+
+  return result;
+}
+
 int main(int argc, char ** argv)
 {
   if (argc != 3) {
@@ -184,6 +255,9 @@ int main(int argc, char ** argv)
   if (action == "Fibonacci") {
     server = receive_goals<test_msgs::action::Fibonacci>(
       node, action, generate_expected_fibonacci_goals(node->get_logger()));
+  } else if (action == "NestedMessage") {
+    server = receive_goals<test_msgs::action::NestedMessage>(
+      node, action, generate_expected_nested_message_goals(node->get_logger()));
   } else {
     fprintf(stderr, "Unknown action type '%s'\n", action.c_str());
     return 1;