Skip to content

Commit

Permalink
update action API
Browse files Browse the repository at this point in the history
  • Loading branch information
dirk-thomas committed Feb 26, 2019
1 parent 4cfbd98 commit 17a7c17
Showing 1 changed file with 15 additions and 17 deletions.
32 changes: 15 additions & 17 deletions test_communication/test/test_action_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -123,13 +123,11 @@ generate_expected_fibonacci_goals(rclcpp::Logger logger)
const auto goal = goal_handle->get_goal();
rclcpp::Rate loop_rate(10);

auto feedback = std::make_shared<
test_msgs::action::Fibonacci::Impl::FeedbackMessage>();
feedback->feedback.sequence.push_back(0);
feedback->feedback.sequence.push_back(1);
auto feedback = std::make_shared<test_msgs::action::Fibonacci::Feedback>();
feedback->sequence.push_back(0);
feedback->sequence.push_back(1);

auto result = std::make_shared<
test_msgs::action::Fibonacci::Impl::GetResultService::Response>();
auto result = std::make_shared<test_msgs::action::Fibonacci::Result>();

if (goal->order <= 0) {
RCLCPP_ERROR(logger, "expected a goal > 0, got %d", goal->order);
Expand All @@ -143,22 +141,22 @@ generate_expected_fibonacci_goals(rclcpp::Logger logger)
}
// Check if the goal was canceled.
if (goal_handle->is_canceling()) {
result->result.sequence = feedback->feedback.sequence;
result->sequence = feedback->sequence;
goal_handle->set_canceled(result);
RCLCPP_INFO(logger, "goal was canceled");
return;
}
// Update the sequence.
feedback->feedback.sequence.push_back(
feedback->feedback.sequence[i] + feedback->feedback.sequence[i - 1]);
feedback->sequence.push_back(
feedback->sequence[i] + feedback->sequence[i - 1]);
// Publish the current state as feedback.
goal_handle->publish_feedback(feedback);
RCLCPP_INFO(logger, "publishing feedback for goal");

loop_rate.sleep();
}

result->result.sequence = feedback->feedback.sequence;
result->sequence = feedback->sequence;
goal_handle->set_succeeded(result);
RCLCPP_INFO(logger, "goal succeeded");
};
Expand Down Expand Up @@ -198,9 +196,9 @@ generate_expected_nested_message_goals(rclcpp::Logger logger)
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::Impl::FeedbackMessage>();
auto feedback = std::make_shared<test_msgs::action::NestedMessage::Feedback>();

auto response = std::make_shared<test_msgs::action::NestedMessage::Impl::GetResultService::Response>();
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);
Expand All @@ -214,22 +212,22 @@ generate_expected_nested_message_goals(rclcpp::Logger logger)
}
// Check if the goal was canceled.
if (goal_handle->is_canceling()) {
response->result.nested_field.int32_value = result_value;
goal_handle->set_canceled(response);
result->nested_field.int32_value = result_value;
goal_handle->set_canceled(result);
RCLCPP_INFO(logger, "goal was canceled");
return;
}
// Update the feedback;
feedback->feedback.nested_different_pkg.sec = feedback_value;
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();
}

response->result.nested_field.int32_value = result_value;
goal_handle->set_succeeded(response);
result->nested_field.int32_value = result_value;
goal_handle->set_succeeded(result);
RCLCPP_INFO(logger, "goal succeeded");
};

Expand Down

0 comments on commit 17a7c17

Please sign in to comment.