diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 9775b5a3e3..914c0965cd 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -52,5 +52,19 @@ create_server( node->get_node_waitables_interface()->add_waitable(action_server, nullptr); return action_server; } + +template +typename Server::SharedPtr +create_server( + rclcpp::Node::SharedPtr node, + const std::string & name, + typename Server::GoalCallback handle_goal, + typename Server::CancelCallback handle_cancel, + typename Server::ExecuteCallback handle_execute, + const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) +{ + return create_server( + node.get(), name, handle_goal, handle_cancel, handle_execute, options); +} } // namespace rclcpp_action #endif // RCLCPP_ACTION__CREATE_SERVER_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 52e4328d07..00cf38bd40 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -109,7 +109,7 @@ class ServerBase : public rclcpp::Waitable RCLCPP_ACTION_PUBLIC virtual std::pair> - call_handle_goal_callback(rcl_action_goal_info_t &, std::shared_ptr request) = 0; + call_handle_goal_callback(std::array &, std::shared_ptr request) = 0; // ServerBase will determine which goal ids are being cancelled, and then call this function for // each goal id. @@ -213,7 +213,7 @@ class Server : public ServerBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server) using GoalCallback = std::function)>; + std::array&, std::shared_ptr)>; using CancelCallback = std::function>)>; using ExecuteCallback = std::function>)>; @@ -244,14 +244,14 @@ class Server : public ServerBase protected: std::pair> - call_handle_goal_callback(rcl_action_goal_info_t & info, std::shared_ptr message) override + call_handle_goal_callback(std::array & uuid, std::shared_ptr message) override { // TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type static_assert( std::is_same::value, "Assuming user fields were merged with goal request fields"); GoalResponse user_response = handle_goal_( - info, std::static_pointer_cast(message)); + uuid, std::static_pointer_cast(message)); auto ros_response = std::make_shared(); ros_response->accepted = GoalResponse::ACCEPT == user_response; diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 2cc9ca038b..8b352cba30 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -208,7 +208,7 @@ ServerBase::execute_goal_request_received() } // Call user's callback, getting the user's response and a ros message to send back - auto response_pair = call_handle_goal_callback(goal_info, message); + auto response_pair = call_handle_goal_callback(uuid, message); ret = rcl_action_send_goal_response( pimpl_->action_server_.get(), diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index d77943f87e..08c4410c48 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -78,7 +78,7 @@ TEST_F(TestServer, construction_and_destruction) using GoalHandle = rclcpp_action::ServerGoalHandle; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - [](rcl_action_goal_info_t &, std::shared_ptr) { + [](std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::REJECT; }, [](std::shared_ptr) { @@ -91,12 +91,12 @@ TEST_F(TestServer, construction_and_destruction) TEST_F(TestServer, handle_goal_called) { auto node = std::make_shared("handle_goal_node", "/rclcpp_action/handle_goal"); - rcl_action_goal_info_t received_info; + std::array received_uuid; - auto handle_goal = [&received_info]( - rcl_action_goal_info_t & info, std::shared_ptr) + auto handle_goal = [&received_uuid]( + std::array & uuid, std::shared_ptr) { - received_info = info; + received_uuid = uuid; return rclcpp_action::GoalResponse::REJECT; }; @@ -127,9 +127,7 @@ TEST_F(TestServer, handle_goal_called) rclcpp::executor::FutureReturnCode::SUCCESS, rclcpp::spin_until_future_complete(node, future)); - for (size_t i = 0; i < 16; ++i) { - EXPECT_EQ(uuid[i], received_info.uuid[i]) << "at idx " << i; - } + ASSERT_EQ(uuid, received_uuid); } TEST_F(TestServer, handle_execute_called) @@ -138,7 +136,7 @@ TEST_F(TestServer, handle_execute_called) const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -173,7 +171,7 @@ TEST_F(TestServer, handle_cancel_called) const std::array uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -213,7 +211,7 @@ TEST_F(TestServer, publish_status_accepted) const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -274,7 +272,7 @@ TEST_F(TestServer, publish_status_canceling) const std::array uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -329,7 +327,7 @@ TEST_F(TestServer, publish_status_canceled) const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -386,7 +384,7 @@ TEST_F(TestServer, publish_status_succeeded) const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -441,7 +439,7 @@ TEST_F(TestServer, publish_status_aborted) const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -496,7 +494,7 @@ TEST_F(TestServer, publish_feedback) const std::array uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -553,7 +551,7 @@ TEST_F(TestServer, get_result) const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; };