Skip to content

Commit

Permalink
rclcpp_action changes that I missed
Browse files Browse the repository at this point in the history
Signed-off-by: William Woodall <william@osrfoundation.org>
  • Loading branch information
wjwwood committed Mar 28, 2024
1 parent d4906d6 commit 16ba34f
Show file tree
Hide file tree
Showing 4 changed files with 12 additions and 15 deletions.
6 changes: 3 additions & 3 deletions rclcpp_action/include/rclcpp_action/server.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -279,19 +279,19 @@ class ServerBase : public rclcpp::Waitable
/// \internal
RCLCPP_ACTION_PUBLIC
void
execute_goal_request_received(std::shared_ptr<void> & data);
execute_goal_request_received(const std::shared_ptr<void> & data);

/// Handle a request to cancel goals on the server
/// \internal
RCLCPP_ACTION_PUBLIC
void
execute_cancel_request_received(std::shared_ptr<void> & data);
execute_cancel_request_received(const std::shared_ptr<void> & data);

/// Handle a request to get the result of an action
/// \internal
RCLCPP_ACTION_PUBLIC
void
execute_result_request_received(std::shared_ptr<void> & data);
execute_result_request_received(const std::shared_ptr<void> & data);

/// Handle a timeout indicating a completed goal should be forgotten by the server
/// \internal
Expand Down
9 changes: 3 additions & 6 deletions rclcpp_action/src/server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -307,7 +307,7 @@ ServerBase::execute(const std::shared_ptr<void> & data)
}

void
ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
ServerBase::execute_goal_request_received(const std::shared_ptr<void> & data)
{
auto shared_ptr = std::static_pointer_cast
<std::tuple<rcl_ret_t, rcl_action_goal_info_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
Expand Down Expand Up @@ -405,11 +405,10 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
// Tell user to start executing action
call_goal_accepted_callback(handle, uuid, message);
}
data.reset();
}

void
ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
ServerBase::execute_cancel_request_received(const std::shared_ptr<void> & data)
{
auto shared_ptr = std::static_pointer_cast
<std::tuple<rcl_ret_t, std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
Expand Down Expand Up @@ -504,11 +503,10 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
if (RCL_RET_OK != ret) {
rclcpp::exceptions::throw_from_rcl_error(ret);
}
data.reset();
}

void
ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
ServerBase::execute_result_request_received(const std::shared_ptr<void> & data)
{
auto shared_ptr = std::static_pointer_cast
<std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>>(data);
Expand Down Expand Up @@ -568,7 +566,6 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
rclcpp::exceptions::throw_from_rcl_error(rcl_ret);
}
}
data.reset();
}

void
Expand Down
6 changes: 3 additions & 3 deletions rclcpp_action/test/test_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -397,13 +397,13 @@ TEST_F(TestClient, is_ready) {
ASSERT_EQ(
RCL_RET_OK,
rcl_wait_set_init(&wait_set, 10, 10, 10, 10, 10, 10, rcl_context, allocator));
ASSERT_NO_THROW(action_client->add_to_wait_set(&wait_set));
EXPECT_TRUE(action_client->is_ready(&wait_set));
ASSERT_NO_THROW(action_client->add_to_wait_set(wait_set));
EXPECT_TRUE(action_client->is_ready(wait_set));

{
auto mock = mocking_utils::patch_and_return(
"lib:rclcpp_action", rcl_action_client_wait_set_get_entities_ready, RCL_RET_ERROR);
EXPECT_THROW(action_client->is_ready(&wait_set), rclcpp::exceptions::RCLError);
EXPECT_THROW(action_client->is_ready(wait_set), rclcpp::exceptions::RCLError);
}
client_node.reset(); // Drop node before action client
}
Expand Down
6 changes: 3 additions & 3 deletions rclcpp_action/test/test_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1107,12 +1107,12 @@ TEST_F(TestGoalRequestServer, is_ready_rcl_error) {
{
EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));
});
EXPECT_NO_THROW(action_server_->add_to_wait_set(&wait_set));
EXPECT_NO_THROW(action_server_->add_to_wait_set(wait_set));

EXPECT_TRUE(action_server_->is_ready(&wait_set));
EXPECT_TRUE(action_server_->is_ready(wait_set));
auto mock = mocking_utils::patch_and_return(
"lib:rclcpp_action", rcl_action_server_wait_set_get_entities_ready, RCL_RET_ERROR);
EXPECT_THROW(action_server_->is_ready(&wait_set), rclcpp::exceptions::RCLError);
EXPECT_THROW(action_server_->is_ready(wait_set), rclcpp::exceptions::RCLError);
}

TEST_F(TestGoalRequestServer, execute_goal_request_received_take_goal_request_errors)
Expand Down

0 comments on commit 16ba34f

Please sign in to comment.