[rclcpp_action] Action client implementation (#594)
* WIP * Removed async_cancel from action ClintGoalHandle API * Added status handler to action client goal handler * Added result handler to action client goal handler * Identation fix * Added get/set for action client goal handler * Changed action client goal handler attrs from rcl to cpp versions * Added check methods to action client goal handler * Removed rcl_client pointer from action client goal handler * Added basic waitable interface to action client * Updated waitable execute from action client * Added throw for rcl calls in action client * Removed duplicated ready flags from action client * Minor fix * Added header to action ClientBaseImpl execute * Mich's update to action client interface * Added trailing suffix to client pimpl attrs * Towards a consistent action client * Misc fixes for the action client * Yet more misc fixes for the action client * Few more fixes and shortcuts to deal with missing type support. * Fixed lint errors in action headers and client * Fixes to action client internal workflow. * Misc fixes to get client example to build * More misck client fixes * Remove debug print * replace logging with throw_from_rcl_error * Wrap result object given by client to user * Fix a couple bugs trying to cancel goals * Use unique_indentifier_msgs * create_client accepts group and removes waitable * Uncrustify fixes * [rclcpp_action] Adds tests for action client. * [WIP] Failing action client tests. * [rclcpp_action] Action client tests passing. * Spin both executors to make tests pass on my machine * Feedback callback uses shared pointer * comment about why make_result_aware is called * Client documentation * Execute one thing at a time * Return nullptr instead of throwing RejectedGoalError * ClientGoalHandle worries about feedback awareness * cpplint + uncrustify * Use node logging interface * ACTION -> ActionT * Make ClientBase constructor protected * Return types on different line * Avoid passing const reference to temporary * Child logger rclcpp_action * Child logger rclcpp_action * possible windows fixes * remove excess space * swap argument order * Misc test additions * Windows independent_bits_engine can't do uint8_t * Windows link issues
This commit is contained in:
Michel Hidalgo
Shane Loretz
parent
33f1e1776c
commit
91167393ea
8 changed files with 1510 additions and 91 deletions
|
|
@ -14,29 +14,407 @@
|
|||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
#include <rcl/time.h>
|
||||
#include <rcl/types.h>
|
||||
|
||||
#include <rcl_action/names.h>
|
||||
#include <rcl_action/default_qos.h>
|
||||
|
||||
#include <rclcpp/clock.hpp>
|
||||
#include <rclcpp/exceptions.hpp>
|
||||
#include <rclcpp/executors.hpp>
|
||||
#include <rclcpp/executors/multi_threaded_executor.hpp>
|
||||
#include <rclcpp/node.hpp>
|
||||
#include <rclcpp/publisher.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp/service.hpp>
|
||||
#include <rclcpp/time.hpp>
|
||||
|
||||
#include <test_msgs/action/fibonacci.hpp>
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
|
||||
#include "rclcpp_action/exceptions.hpp"
|
||||
#include "rclcpp_action/create_client.hpp"
|
||||
#include "rclcpp_action/client.hpp"
|
||||
#include "rclcpp_action/types.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
class TestClient : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
using ActionType = test_msgs::action::Fibonacci;
|
||||
using ActionGoal = ActionType::Goal;
|
||||
using ActionGoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
|
||||
using ActionGoalRequestService = ActionType::GoalRequestService;
|
||||
using ActionGoalRequest = ActionGoalRequestService::Request;
|
||||
using ActionGoalResponse = ActionGoalRequestService::Response;
|
||||
using ActionGoalResultService = ActionType::GoalResultService;
|
||||
using ActionGoalResultRequest = ActionGoalResultService::Request;
|
||||
using ActionGoalResultResponse = ActionGoalResultService::Response;
|
||||
using ActionCancelGoalService = ActionType::CancelGoalService;
|
||||
using ActionCancelGoalRequest = ActionType::CancelGoalService::Request;
|
||||
using ActionCancelGoalResponse = ActionType::CancelGoalService::Response;
|
||||
using ActionStatusMessage = ActionType::GoalStatusMessage;
|
||||
using ActionFeedbackMessage = ActionType::Feedback;
|
||||
using ActionFeedback = ActionType::Feedback;
|
||||
|
||||
static void SetUpTestCase()
|
||||
{
|
||||
rclcpp::init(0, nullptr);
|
||||
}
|
||||
|
||||
void SetUp()
|
||||
{
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
|
||||
server_node = std::make_shared<rclcpp::Node>(server_node_name, namespace_name);
|
||||
|
||||
char * goal_service_name = nullptr;
|
||||
rcl_ret_t ret = rcl_action_get_goal_service_name(
|
||||
action_name, allocator, &goal_service_name);
|
||||
ASSERT_EQ(RCL_RET_OK, ret);
|
||||
goal_service = server_node->create_service<ActionGoalRequestService>(
|
||||
goal_service_name,
|
||||
[this](
|
||||
const ActionGoalRequest::SharedPtr request,
|
||||
ActionGoalResponse::SharedPtr response)
|
||||
{
|
||||
response->stamp = clock.now();
|
||||
response->accepted = (request->order >= 0);
|
||||
if (response->accepted) {
|
||||
goals[request->uuid] = {request, response};
|
||||
}
|
||||
});
|
||||
ASSERT_TRUE(goal_service != nullptr);
|
||||
allocator.deallocate(goal_service_name, allocator.state);
|
||||
|
||||
char * result_service_name = nullptr;
|
||||
ret = rcl_action_get_result_service_name(
|
||||
action_name, allocator, &result_service_name);
|
||||
ASSERT_EQ(RCL_RET_OK, ret);
|
||||
result_service = server_node->create_service<ActionGoalResultService>(
|
||||
result_service_name,
|
||||
[this](
|
||||
const ActionGoalResultRequest::SharedPtr request,
|
||||
ActionGoalResultResponse::SharedPtr response)
|
||||
{
|
||||
if (goals.count(request->uuid) == 1) {
|
||||
auto goal_request = goals[request->uuid].first;
|
||||
auto goal_response = goals[request->uuid].second;
|
||||
ActionStatusMessage status_message;
|
||||
rclcpp_action::GoalStatus goal_status;
|
||||
goal_status.goal_info.goal_id.uuid = goal_request->uuid;
|
||||
goal_status.goal_info.stamp = goal_response->stamp;
|
||||
goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING;
|
||||
status_message.status_list.push_back(goal_status);
|
||||
status_publisher->publish(status_message);
|
||||
client_executor.spin_once();
|
||||
ActionFeedbackMessage feedback_message;
|
||||
feedback_message.uuid = goal_request->uuid;
|
||||
feedback_message.sequence.push_back(0);
|
||||
feedback_publisher->publish(feedback_message);
|
||||
client_executor.spin_once();
|
||||
if (goal_request->order > 0) {
|
||||
feedback_message.sequence.push_back(1);
|
||||
feedback_publisher->publish(feedback_message);
|
||||
client_executor.spin_once();
|
||||
for (int i = 1; i < goal_request->order; ++i) {
|
||||
feedback_message.sequence.push_back(
|
||||
feedback_message.sequence[i] + feedback_message.sequence[i - 1]);
|
||||
feedback_publisher->publish(feedback_message);
|
||||
client_executor.spin_once();
|
||||
}
|
||||
}
|
||||
goal_status.status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
|
||||
status_message.status_list[0] = goal_status;
|
||||
status_publisher->publish(status_message);
|
||||
client_executor.spin_once();
|
||||
response->sequence = feedback_message.sequence;
|
||||
response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
|
||||
goals.erase(request->uuid);
|
||||
} else {
|
||||
response->status = rclcpp_action::GoalStatus::STATUS_UNKNOWN;
|
||||
}
|
||||
});
|
||||
ASSERT_TRUE(result_service != nullptr);
|
||||
allocator.deallocate(result_service_name, allocator.state);
|
||||
|
||||
char * cancel_service_name = nullptr;
|
||||
ret = rcl_action_get_cancel_service_name(
|
||||
action_name, allocator, &cancel_service_name);
|
||||
ASSERT_EQ(RCL_RET_OK, ret);
|
||||
cancel_service = server_node->create_service<ActionCancelGoalService>(
|
||||
cancel_service_name,
|
||||
[this](
|
||||
const ActionCancelGoalRequest::SharedPtr request,
|
||||
ActionCancelGoalResponse::SharedPtr response)
|
||||
{
|
||||
rclcpp_action::GoalID zero_uuid;
|
||||
std::fill(zero_uuid.begin(), zero_uuid.end(), 0u);
|
||||
const rclcpp::Time cancel_stamp = request->goal_info.stamp;
|
||||
bool cancel_all = (
|
||||
request->goal_info.goal_id.uuid == zero_uuid &&
|
||||
cancel_stamp == zero_stamp);
|
||||
ActionStatusMessage status_message;
|
||||
auto it = goals.begin();
|
||||
while (it != goals.end()) {
|
||||
auto goal_request = it->second.first;
|
||||
auto goal_response = it->second.second;
|
||||
const rclcpp::Time goal_stamp = goal_response->stamp;
|
||||
bool cancel_this = (
|
||||
request->goal_info.goal_id.uuid == goal_request->uuid ||
|
||||
cancel_stamp > goal_stamp);
|
||||
if (cancel_all || cancel_this) {
|
||||
rclcpp_action::GoalStatus goal_status;
|
||||
goal_status.goal_info.goal_id.uuid = goal_request->uuid;
|
||||
goal_status.goal_info.stamp = goal_response->stamp;
|
||||
goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED;
|
||||
status_message.status_list.push_back(goal_status);
|
||||
response->goals_canceling.push_back(goal_status.goal_info);
|
||||
it = goals.erase(it);
|
||||
} else {
|
||||
++it;
|
||||
}
|
||||
}
|
||||
status_publisher->publish(status_message);
|
||||
client_executor.spin_once();
|
||||
});
|
||||
ASSERT_TRUE(cancel_service != nullptr);
|
||||
allocator.deallocate(cancel_service_name, allocator.state);
|
||||
|
||||
char * feedback_topic_name = nullptr;
|
||||
ret = rcl_action_get_feedback_topic_name(
|
||||
action_name, allocator, &feedback_topic_name);
|
||||
ASSERT_EQ(RCL_RET_OK, ret);
|
||||
feedback_publisher = server_node->create_publisher<ActionFeedbackMessage>(feedback_topic_name);
|
||||
ASSERT_TRUE(feedback_publisher != nullptr);
|
||||
allocator.deallocate(feedback_topic_name, allocator.state);
|
||||
|
||||
char * status_topic_name = nullptr;
|
||||
ret = rcl_action_get_status_topic_name(
|
||||
action_name, allocator, &status_topic_name);
|
||||
ASSERT_EQ(RCL_RET_OK, ret);
|
||||
status_publisher = server_node->create_publisher<ActionStatusMessage>(
|
||||
status_topic_name, rcl_action_qos_profile_status_default);
|
||||
ASSERT_TRUE(status_publisher != nullptr);
|
||||
allocator.deallocate(status_topic_name, allocator.state);
|
||||
server_executor.add_node(server_node);
|
||||
|
||||
client_node = std::make_shared<rclcpp::Node>(client_node_name, namespace_name);
|
||||
client_executor.add_node(client_node);
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(clock.get_clock_handle()));
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(1)));
|
||||
}
|
||||
|
||||
void Teardown()
|
||||
{
|
||||
status_publisher.reset();
|
||||
feedback_publisher.reset();
|
||||
cancel_service.reset();
|
||||
result_service.reset();
|
||||
goal_service.reset();
|
||||
server_node.reset();
|
||||
client_node.reset();
|
||||
}
|
||||
|
||||
template<typename FutureT>
|
||||
void dual_spin_until_future_complete(std::shared_future<FutureT> & future)
|
||||
{
|
||||
std::future_status status;
|
||||
do {
|
||||
server_executor.spin_some();
|
||||
client_executor.spin_some();
|
||||
status = future.wait_for(std::chrono::seconds(0));
|
||||
} while (std::future_status::ready != status);
|
||||
}
|
||||
|
||||
rclcpp::Clock clock{RCL_ROS_TIME};
|
||||
const rclcpp::Time zero_stamp{0, 0, RCL_ROS_TIME};
|
||||
|
||||
rclcpp::Node::SharedPtr server_node;
|
||||
rclcpp::executors::SingleThreadedExecutor server_executor;
|
||||
rclcpp::Node::SharedPtr client_node;
|
||||
rclcpp::executors::SingleThreadedExecutor client_executor;
|
||||
const char * const server_node_name{"fibonacci_action_test_server"};
|
||||
const char * const client_node_name{"fibonacci_action_test_client"};
|
||||
const char * const namespace_name{"/rclcpp_action/test/client"};
|
||||
const char * const action_name{"fibonacci_test"};
|
||||
|
||||
std::map<
|
||||
rclcpp_action::GoalID,
|
||||
std::pair<
|
||||
typename ActionGoalRequest::SharedPtr,
|
||||
typename ActionGoalResponse::SharedPtr>> goals;
|
||||
typename rclcpp::Service<ActionGoalRequestService>::SharedPtr goal_service;
|
||||
typename rclcpp::Service<ActionGoalResultService>::SharedPtr result_service;
|
||||
typename rclcpp::Service<ActionCancelGoalService>::SharedPtr cancel_service;
|
||||
typename rclcpp::Publisher<ActionFeedbackMessage>::SharedPtr feedback_publisher;
|
||||
typename rclcpp::Publisher<ActionStatusMessage>::SharedPtr status_publisher;
|
||||
};
|
||||
|
||||
TEST_F(TestClient, construction_and_destruction)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp::Node>("my_node", "/rclcpp_action/test/client");
|
||||
|
||||
auto ac = rclcpp_action::create_client<test_msgs::action::Fibonacci>(node.get(), "fibonacci");
|
||||
(void)ac;
|
||||
ASSERT_NO_THROW(rclcpp_action::create_client<ActionType>(client_node, action_name).reset());
|
||||
}
|
||||
|
||||
TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result)
|
||||
{
|
||||
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
|
||||
|
||||
ActionGoal bad_goal;
|
||||
bad_goal.order = -5;
|
||||
auto future_goal_handle = action_client->async_send_goal(bad_goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle);
|
||||
EXPECT_EQ(nullptr, future_goal_handle.get().get());
|
||||
|
||||
ActionGoal good_goal;
|
||||
good_goal.order = 5;
|
||||
future_goal_handle = action_client->async_send_goal(good_goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle);
|
||||
auto goal_handle = future_goal_handle.get();
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
|
||||
EXPECT_FALSE(goal_handle->is_feedback_aware());
|
||||
EXPECT_FALSE(goal_handle->is_result_aware());
|
||||
EXPECT_THROW(goal_handle->async_result(), rclcpp_action::exceptions::UnawareGoalHandleError);
|
||||
}
|
||||
|
||||
TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result)
|
||||
{
|
||||
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
|
||||
|
||||
ActionGoal goal;
|
||||
goal.order = 5;
|
||||
auto future_goal_handle = action_client->async_send_goal(goal);
|
||||
dual_spin_until_future_complete(future_goal_handle);
|
||||
auto goal_handle = future_goal_handle.get();
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
|
||||
EXPECT_FALSE(goal_handle->is_feedback_aware());
|
||||
EXPECT_TRUE(goal_handle->is_result_aware());
|
||||
auto future_result = goal_handle->async_result();
|
||||
dual_spin_until_future_complete(future_result);
|
||||
auto result = future_result.get();
|
||||
ASSERT_EQ(6ul, result.response->sequence.size());
|
||||
EXPECT_EQ(0, result.response->sequence[0]);
|
||||
EXPECT_EQ(1, result.response->sequence[1]);
|
||||
EXPECT_EQ(5, result.response->sequence[5]);
|
||||
}
|
||||
|
||||
TEST_F(TestClient, async_send_goal_with_feedback_and_result)
|
||||
{
|
||||
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
|
||||
|
||||
ActionGoal goal;
|
||||
goal.order = 4;
|
||||
int feedback_count = 0;
|
||||
auto future_goal_handle = action_client->async_send_goal(
|
||||
goal,
|
||||
[&feedback_count](
|
||||
typename ActionGoalHandle::SharedPtr goal_handle,
|
||||
const std::shared_ptr<const ActionFeedback> feedback) mutable
|
||||
{
|
||||
(void)goal_handle;
|
||||
(void)feedback;
|
||||
feedback_count++;
|
||||
});
|
||||
dual_spin_until_future_complete(future_goal_handle);
|
||||
auto goal_handle = future_goal_handle.get();
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
|
||||
EXPECT_TRUE(goal_handle->is_feedback_aware());
|
||||
EXPECT_TRUE(goal_handle->is_result_aware());
|
||||
auto future_result = goal_handle->async_result();
|
||||
dual_spin_until_future_complete(future_result);
|
||||
auto result = future_result.get();
|
||||
|
||||
ASSERT_EQ(5u, result.response->sequence.size());
|
||||
EXPECT_EQ(3, result.response->sequence.back());
|
||||
EXPECT_EQ(5, feedback_count);
|
||||
}
|
||||
|
||||
TEST_F(TestClient, async_cancel_one_goal)
|
||||
{
|
||||
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
|
||||
|
||||
ActionGoal goal;
|
||||
goal.order = 5;
|
||||
auto future_goal_handle = action_client->async_send_goal(goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle);
|
||||
auto goal_handle = future_goal_handle.get();
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
|
||||
|
||||
auto future_cancel = action_client->async_cancel_goal(goal_handle);
|
||||
dual_spin_until_future_complete(future_cancel);
|
||||
bool goal_canceled = future_cancel.get();
|
||||
EXPECT_TRUE(goal_canceled);
|
||||
}
|
||||
|
||||
TEST_F(TestClient, async_cancel_all_goals)
|
||||
{
|
||||
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
|
||||
|
||||
ActionGoal goal;
|
||||
goal.order = 6;
|
||||
auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle0);
|
||||
auto goal_handle0 = future_goal_handle0.get();
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
|
||||
|
||||
goal.order = 8;
|
||||
auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle1);
|
||||
auto goal_handle1 = future_goal_handle1.get();
|
||||
|
||||
if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) {
|
||||
goal_handle0.swap(goal_handle1);
|
||||
}
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));
|
||||
|
||||
auto future_cancel_all = action_client->async_cancel_all_goals();
|
||||
dual_spin_until_future_complete(future_cancel_all);
|
||||
auto cancel_response = future_cancel_all.get();
|
||||
|
||||
ASSERT_EQ(2ul, cancel_response->goals_canceling.size());
|
||||
EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
|
||||
EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid);
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
|
||||
}
|
||||
|
||||
TEST_F(TestClient, async_cancel_some_goals)
|
||||
{
|
||||
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
|
||||
|
||||
ActionGoal goal;
|
||||
goal.order = 6;
|
||||
auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle0);
|
||||
auto goal_handle0 = future_goal_handle0.get();
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
|
||||
|
||||
goal.order = 8;
|
||||
auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle1);
|
||||
auto goal_handle1 = future_goal_handle1.get();
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));
|
||||
|
||||
auto future_cancel_some =
|
||||
action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp());
|
||||
dual_spin_until_future_complete(future_cancel_some);
|
||||
auto cancel_response = future_cancel_some.get();
|
||||
|
||||
ASSERT_EQ(1ul, cancel_response->goals_canceling.size());
|
||||
EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue