diff --git a/rclcpp/minimal_action_client/CMakeLists.txt b/rclcpp/minimal_action_client/CMakeLists.txt index 1dab0e65..4404a0f1 100644 --- a/rclcpp/minimal_action_client/CMakeLists.txt +++ b/rclcpp/minimal_action_client/CMakeLists.txt @@ -15,6 +15,12 @@ find_package(example_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +add_executable(action_client_member_functions member_functions.cpp) +ament_target_dependencies(action_client_member_functions + "rclcpp" + "rclcpp_action" + "example_interfaces") + add_executable(action_client_not_composable not_composable.cpp) ament_target_dependencies(action_client_not_composable "rclcpp" @@ -34,6 +40,7 @@ ament_target_dependencies(action_client_not_composable_with_feedback "example_interfaces") install(TARGETS + action_client_member_functions action_client_not_composable action_client_not_composable_with_cancel action_client_not_composable_with_feedback diff --git a/rclcpp/minimal_action_client/member_functions.cpp b/rclcpp/minimal_action_client/member_functions.cpp new file mode 100644 index 00000000..782af5d5 --- /dev/null +++ b/rclcpp/minimal_action_client/member_functions.cpp @@ -0,0 +1,143 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include "example_interfaces/action/fibonacci.hpp" +#include "rclcpp/rclcpp.hpp" +// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp' +#include "rclcpp_action/rclcpp_action.hpp" + +class MinimalActionClient : public rclcpp::Node +{ +public: + using Fibonacci = example_interfaces::action::Fibonacci; + using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle; + + MinimalActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + : Node("minimal_action_client", node_options), goal_done_(false) + { + this->client_ptr_ = rclcpp_action::create_client( + this->get_node_base_interface(), + this->get_node_graph_interface(), + this->get_node_logging_interface(), + this->get_node_waitables_interface(), + "fibonacci"); + + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&MinimalActionClient::send_goal, this)); + } + + bool is_goal_done() const + { + return this->goal_done_; + } + + void send_goal() + { + using namespace std::placeholders; + + this->timer_->cancel(); + + this->goal_done_ = false; + + if (!this->client_ptr_) { + RCLCPP_ERROR(this->get_logger(), "Action client not initialized"); + } + + if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + this->goal_done_ = true; + return; + } + + auto goal_msg = Fibonacci::Goal(); + goal_msg.order = 10; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&MinimalActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&MinimalActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&MinimalActionClient::result_callback, this, _1); + auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + } + +private: + rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::TimerBase::SharedPtr timer_; + bool goal_done_; + + void goal_response_callback(std::shared_future future) + { + auto goal_handle = future.get(); + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } + } + + void feedback_callback( + GoalHandleFibonacci::SharedPtr, + const std::shared_ptr feedback) + { + RCLCPP_INFO( + this->get_logger(), + "Next number in sequence received: %" PRId64, + feedback->sequence.back()); + } + + void result_callback(const GoalHandleFibonacci::WrappedResult & result) + { + this->goal_done_ = true; + switch(result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Result received"); + for (auto number : result.result->sequence) { + RCLCPP_INFO(this->get_logger(), "%" PRId64, number); + } + } +}; // class MinimalActionClient + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto action_client = std::make_shared(); + + while (!action_client->is_goal_done()) { + rclcpp::spin_some(action_client); + } + + rclcpp::shutdown(); + return 0; +} diff --git a/rclcpp/minimal_action_client/not_composable_with_feedback.cpp b/rclcpp/minimal_action_client/not_composable_with_feedback.cpp index b42e4e2c..b3199ed4 100644 --- a/rclcpp/minimal_action_client/not_composable_with_feedback.cpp +++ b/rclcpp/minimal_action_client/not_composable_with_feedback.cpp @@ -50,7 +50,9 @@ int main(int argc, char ** argv) RCLCPP_INFO(g_node->get_logger(), "Sending goal"); // Ask server to achieve some goal and wait until it's accepted - auto goal_handle_future = action_client->async_send_goal(goal_msg, feedback_callback); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.feedback_callback = feedback_callback; + auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); if (rclcpp::spin_until_future_complete(g_node, goal_handle_future) != rclcpp::executor::FutureReturnCode::SUCCESS) diff --git a/rclcpp/minimal_action_server/CMakeLists.txt b/rclcpp/minimal_action_server/CMakeLists.txt index a4c488ee..6c0a09e5 100644 --- a/rclcpp/minimal_action_server/CMakeLists.txt +++ b/rclcpp/minimal_action_server/CMakeLists.txt @@ -15,13 +15,21 @@ find_package(example_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +add_executable(action_server_member_functions member_functions.cpp) +ament_target_dependencies(action_server_member_functions + "rclcpp" + "rclcpp_action" + "example_interfaces") + add_executable(action_server_not_composable not_composable.cpp) ament_target_dependencies(action_server_not_composable "rclcpp" "rclcpp_action" "example_interfaces") -install(TARGETS action_server_not_composable +install(TARGETS + action_server_not_composable + action_server_member_functions DESTINATION lib/${PROJECT_NAME}) ament_package() diff --git a/rclcpp/minimal_action_server/member_functions.cpp b/rclcpp/minimal_action_server/member_functions.cpp new file mode 100644 index 00000000..a24cab81 --- /dev/null +++ b/rclcpp/minimal_action_server/member_functions.cpp @@ -0,0 +1,125 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include "example_interfaces/action/fibonacci.hpp" +#include "rclcpp/rclcpp.hpp" +// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp' +#include "rclcpp_action/rclcpp_action.hpp" + +class MinimalActionServer : public rclcpp::Node +{ +public: + using Fibonacci = example_interfaces::action::Fibonacci; + using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle; + + MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("minimal_action_server", options) + { + using namespace std::placeholders; + + this->action_server_ = rclcpp_action::create_server( + this->get_node_base_interface(), + this->get_node_clock_interface(), + this->get_node_logging_interface(), + this->get_node_waitables_interface(), + "fibonacci", + std::bind(&MinimalActionServer::handle_goal, this, _1, _2), + std::bind(&MinimalActionServer::handle_cancel, this, _1), + std::bind(&MinimalActionServer::handle_accepted, this, _1)); + } + +private: + rclcpp_action::Server::SharedPtr action_server_; + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); + (void)uuid; + // Let's reject sequences that are over 9000 + if (goal->order > 9000) + { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void execute(const std::shared_ptr goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Executing goal"); + rclcpp::Rate loop_rate(1); + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto& sequence = feedback->sequence; + sequence.push_back(0); + sequence.push_back(1); + auto result = std::make_shared(); + + for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) + { + // Check if there is a cancel request + if (goal_handle->is_canceling()) + { + result->sequence = sequence; + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "Goal Canceled"); + return; + } + // Update sequence + sequence.push_back(sequence[i] + sequence[i - 1]); + // Publish feedback + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), "Publish Feedback"); + + loop_rate.sleep(); + } + + // Check if goal is done + if (rclcpp::ok()) { + result->sequence = sequence; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal Suceeded"); + } + } + + void handle_accepted(const std::shared_ptr goal_handle) + { + using namespace std::placeholders; + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&MinimalActionServer::execute, this, _1), goal_handle}.detach(); + } +}; // class MinimalActionServer + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto action_server = std::make_shared(); + + rclcpp::spin(action_server); + + rclcpp::shutdown(); + return 0; +} diff --git a/rclcpp/minimal_action_server/not_composable.cpp b/rclcpp/minimal_action_server/not_composable.cpp index 0b95f718..638a5a79 100644 --- a/rclcpp/minimal_action_server/not_composable.cpp +++ b/rclcpp/minimal_action_server/not_composable.cpp @@ -23,7 +23,7 @@ using Fibonacci = example_interfaces::action::Fibonacci; using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle; rclcpp_action::GoalResponse handle_goal( - const std::array & uuid, std::shared_ptr goal) + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) { RCLCPP_INFO(rclcpp::get_logger("server"), "Got goal request with order %d", goal->order); (void)uuid;