Skip to content

Commit

Permalink
adapt to action implicit changes (#602)
Browse files Browse the repository at this point in the history
  • Loading branch information
@sloretz
sloretz committed Dec 7, 2018
1 parent fe09d93 commit ef2014a
Show file tree
Hide file tree
Showing 5 changed files with 31 additions and 29 deletions.
10 changes: 5 additions & 5 deletions rclcpp_action/include/rclcpp_action/client.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -265,7 +265,7 @@ class Client : public ClientBase
// goal_request->goal_id = this->generate_goal_id();
// goal_request->goal = goal;
auto goal_request = std::make_shared<GoalRequest>(goal);
goal_request->uuid = this->generate_goal_id();
goal_request->action_goal_id.uuid = this->generate_goal_id();
this->send_goal_request(
std::static_pointer_cast<void>(goal_request),
[this, goal_request, callback, ignore_result, promise](
Expand All @@ -279,7 +279,7 @@ class Client : public ClientBase
}
GoalInfo goal_info;
// goal_info.goal_id = goal_request->goal_id;
goal_info.goal_id.uuid = goal_request->uuid;
goal_info.goal_id.uuid = goal_request->action_goal_id.uuid;
goal_info.stamp = goal_response->stamp;
// Do not use std::make_shared as friendship cannot be forwarded.
std::shared_ptr<GoalHandle> goal_handle(new GoalHandle(goal_info, callback));
Expand Down Expand Up @@ -419,7 +419,7 @@ class Client : public ClientBase
typename Feedback::SharedPtr feedback_message =
std::static_pointer_cast<Feedback>(message);
// const GoalID & goal_id = feedback_message->goal_id;
const GoalID & goal_id = feedback_message->uuid;
const GoalID & goal_id = feedback_message->action_goal_id.uuid;
if (goal_handles_.count(goal_id) == 0) {
RCLCPP_DEBUG(
this->get_logger(),
Expand Down Expand Up @@ -475,7 +475,7 @@ class Client : public ClientBase
using GoalResultRequest = typename ActionT::GoalResultService::Request;
auto goal_result_request = std::make_shared<GoalResultRequest>();
// goal_result_request.goal_id = goal_handle->get_goal_id();
goal_result_request->uuid = goal_handle->get_goal_id();
goal_result_request->action_goal_id.uuid = goal_handle->get_goal_id();
this->send_result_request(
std::static_pointer_cast<void>(goal_result_request),
[goal_handle, this](std::shared_ptr<void> response) mutable
Expand All @@ -485,7 +485,7 @@ class Client : public ClientBase
using GoalResultResponse = typename ActionT::GoalResultService::Response;
result.response = std::static_pointer_cast<GoalResultResponse>(response);
result.goal_id = goal_handle->get_goal_id();
result.code = static_cast<ResultCode>(result.response->status);
result.code = static_cast<ResultCode>(result.response->action_status);
goal_handle->set_result(result);
std::lock_guard<std::mutex> lock(goal_handles_mutex_);
goal_handles_.erase(goal_handle->get_goal_id());
Expand Down
8 changes: 5 additions & 3 deletions rclcpp_action/include/rclcpp_action/server.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -434,7 +434,8 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
GoalID
get_goal_id_from_goal_request(void * message) override
{
return static_cast<typename ActionT::GoalRequestService::Request *>(message)->uuid;
return
static_cast<typename ActionT::GoalRequestService::Request *>(message)->action_goal_id.uuid;
}

/// \internal
Expand All @@ -448,7 +449,8 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
GoalID
get_goal_id_from_result_request(void * message) override
{
return static_cast<typename ActionT::GoalResultService::Request *>(message)->uuid;
return
static_cast<typename ActionT::GoalResultService::Request *>(message)->action_goal_id.uuid;
}

/// \internal
Expand All @@ -463,7 +465,7 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override
{
auto result = std::make_shared<typename ActionT::GoalResultService::Response>();
result->status = status;
result->action_status = status;
return std::static_pointer_cast<void>(result);
}

Expand Down
10 changes: 5 additions & 5 deletions rclcpp_action/include/rclcpp_action/server_goal_handle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,7 @@ class ServerGoalHandle : public ServerGoalHandleBase
void
publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
{
feedback_msg->uuid = uuid_;
feedback_msg->action_goal_id.uuid = uuid_;
publish_feedback_(feedback_msg);
}

Expand All @@ -165,7 +165,7 @@ class ServerGoalHandle : public ServerGoalHandleBase
set_aborted(typename ActionT::Result::SharedPtr result_msg)
{
_set_aborted();
result_msg->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
on_terminal_state_(uuid_, result_msg);
}

Expand All @@ -182,7 +182,7 @@ class ServerGoalHandle : public ServerGoalHandleBase
set_succeeded(typename ActionT::Result::SharedPtr result_msg)
{
_set_succeeded();
result_msg->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
on_terminal_state_(uuid_, result_msg);
}

Expand All @@ -199,7 +199,7 @@ class ServerGoalHandle : public ServerGoalHandleBase
set_canceled(typename ActionT::Result::SharedPtr result_msg)
{
_set_canceled();
result_msg->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
on_terminal_state_(uuid_, result_msg);
}

Expand Down Expand Up @@ -234,7 +234,7 @@ class ServerGoalHandle : public ServerGoalHandleBase
// Cancel goal if handle was allowed to destruct without reaching a terminal state
if (try_canceling()) {
auto null_result = std::make_shared<typename ActionT::Result>();
null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
null_result->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
on_terminal_state_(uuid_, null_result);
}
}
Expand Down
22 changes: 11 additions & 11 deletions rclcpp_action/test/test_client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ class TestClient : public ::testing::Test
response->stamp = clock.now();
response->accepted = (request->order >= 0);
if (response->accepted) {
goals[request->uuid] = {request, response};
goals[request->action_goal_id.uuid] = {request, response};
}
});
ASSERT_TRUE(goal_service != nullptr);
Expand All @@ -106,19 +106,19 @@ class TestClient : public ::testing::Test
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;
if (goals.count(request->action_goal_id.uuid) == 1) {
auto goal_request = goals[request->action_goal_id.uuid].first;
auto goal_response = goals[request->action_goal_id.uuid].second;
ActionStatusMessage status_message;
rclcpp_action::GoalStatus goal_status;
goal_status.goal_info.goal_id.uuid = goal_request->uuid;
goal_status.goal_info.goal_id.uuid = goal_request->action_goal_id.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.action_goal_id.uuid = goal_request->action_goal_id.uuid;
feedback_message.sequence.push_back(0);
feedback_publisher->publish(feedback_message);
client_executor.spin_once();
Expand All @@ -138,10 +138,10 @@ class TestClient : public ::testing::Test
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);
response->action_status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
goals.erase(request->action_goal_id.uuid);
} else {
response->status = rclcpp_action::GoalStatus::STATUS_UNKNOWN;
response->action_status = rclcpp_action::GoalStatus::STATUS_UNKNOWN;
}
});
ASSERT_TRUE(result_service != nullptr);
Expand Down Expand Up @@ -170,11 +170,11 @@ class TestClient : public ::testing::Test
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 ||
request->goal_info.goal_id.uuid == goal_request->action_goal_id.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.goal_id.uuid = goal_request->action_goal_id.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);
Expand Down
10 changes: 5 additions & 5 deletions rclcpp_action/test/test_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ class TestServer : public ::testing::Test
throw std::runtime_error("send goal service didn't become available");
}
auto request = std::make_shared<Fibonacci::GoalRequestService::Request>();
request->uuid = uuid;
request->action_goal_id.uuid = uuid;
auto future = client->async_send_request(request);
if (rclcpp::executor::FutureReturnCode::SUCCESS !=
rclcpp::spin_until_future_complete(node, future))
Expand Down Expand Up @@ -122,7 +122,7 @@ TEST_F(TestServer, handle_goal_called)
auto request = std::make_shared<Fibonacci::GoalRequestService::Request>();

const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
request->uuid = uuid;
request->action_goal_id.uuid = uuid;

auto future = client->async_send_request(request);
ASSERT_EQ(
Expand Down Expand Up @@ -545,7 +545,7 @@ TEST_F(TestServer, publish_feedback)
ASSERT_EQ(1u, received_msgs.size());
auto & msg = received_msgs.back();
ASSERT_EQ(sent_message->sequence, msg->sequence);
ASSERT_EQ(uuid, msg->uuid);
ASSERT_EQ(uuid, msg->action_goal_id.uuid);
}

TEST_F(TestServer, get_result)
Expand Down Expand Up @@ -587,7 +587,7 @@ TEST_F(TestServer, get_result)
throw std::runtime_error("get result service didn't become available");
}
auto request = std::make_shared<Fibonacci::GoalResultService::Request>();
request->uuid = uuid;
request->action_goal_id.uuid = uuid;
auto future = result_client->async_send_request(request);

// Send a result
Expand All @@ -600,7 +600,7 @@ TEST_F(TestServer, get_result)
rclcpp::spin_until_future_complete(node, future));

auto response = future.get();
EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status);
EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->action_status);
EXPECT_EQ(result->sequence, response->sequence);
}

Expand Down

0 comments on commit ef2014a

Please sign in to comment.