From 2447bf8b3292991089508ad7f2dc3010d5ad6a7f Mon Sep 17 00:00:00 2001 From: Tirine Date: Sat, 29 Jan 2022 16:21:04 +0200 Subject: [PATCH] Added random_generator package, including an action interface, action client and action server, all in one. --- .../actions/random_generator/CMakeLists.txt | 85 +++++++++++++ rclcpp/actions/random_generator/README.md | 6 + .../random_generator/action/Randomizer.action | 8 ++ .../random_generator/visibility_control.h | 44 +++++++ rclcpp/actions/random_generator/package.xml | 27 ++++ .../src/randomizer_action_client.cpp | 116 ++++++++++++++++++ .../src/randomizer_action_server.cpp | 114 +++++++++++++++++ 7 files changed, 400 insertions(+) create mode 100644 rclcpp/actions/random_generator/CMakeLists.txt create mode 100644 rclcpp/actions/random_generator/README.md create mode 100644 rclcpp/actions/random_generator/action/Randomizer.action create mode 100644 rclcpp/actions/random_generator/include/random_generator/visibility_control.h create mode 100644 rclcpp/actions/random_generator/package.xml create mode 100644 rclcpp/actions/random_generator/src/randomizer_action_client.cpp create mode 100644 rclcpp/actions/random_generator/src/randomizer_action_server.cpp diff --git a/rclcpp/actions/random_generator/CMakeLists.txt b/rclcpp/actions/random_generator/CMakeLists.txt new file mode 100644 index 00000000..b164c89f --- /dev/null +++ b/rclcpp/actions/random_generator/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.8) +project(random_generator) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# Action Interface +rosidl_generate_interfaces(${PROJECT_NAME} + "action/Randomizer.action" +) +ament_export_dependencies( + rosidl_default_runtime +) + +# Action Client +add_library(action_client SHARED + src/randomizer_action_client.cpp) +target_include_directories(action_client PRIVATE + $ + $) +target_compile_definitions(action_client + PRIVATE "RANDOM_GENERATOR_BUILDING_DLL") +ament_target_dependencies(action_client + "rclcpp" + "rclcpp_action" + "rclcpp_components") +rclcpp_components_register_node(action_client + PLUGIN "random_generator::RandomizerActionClient" + EXECUTABLE randomizer_action_client) +install(TARGETS action_client + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +# Action Server +add_library(action_server SHARED + src/randomizer_action_server.cpp) +target_include_directories(action_server PRIVATE + $ + $) +target_compile_definitions(action_server + PRIVATE "RANDOM_GENERATOR_BUILDING_DLL") +ament_target_dependencies(action_server + "rclcpp" + "rclcpp_action" + "rclcpp_components") +rclcpp_components_register_node(action_server + PLUGIN "random_generator::RandomizerActionServer" + EXECUTABLE randomizer_action_server) +install(TARGETS action_server + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +# Include the action interface to the client and server, both the library and executable +rosidl_target_interfaces(action_server + ${PROJECT_NAME} "rosidl_typesupport_cpp") +rosidl_target_interfaces(action_client + ${PROJECT_NAME} "rosidl_typesupport_cpp") +rosidl_target_interfaces(randomizer_action_client + ${PROJECT_NAME} "rosidl_typesupport_cpp") +rosidl_target_interfaces(randomizer_action_server + ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# Finally +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/rclcpp/actions/random_generator/README.md b/rclcpp/actions/random_generator/README.md new file mode 100644 index 00000000..c4bef3f8 --- /dev/null +++ b/rclcpp/actions/random_generator/README.md @@ -0,0 +1,6 @@ +# A single package with the action interface and the client and server. + +This package contains an few example which show how to create a single package with the interface, client and server, all in one. + +The main difference between the msg/srv interface that was included into a package with it's client and service is that an action client and server needs to have both the library and executable included in the rosidl_typesupport_cpp in the CMakeLists file. + diff --git a/rclcpp/actions/random_generator/action/Randomizer.action b/rclcpp/actions/random_generator/action/Randomizer.action new file mode 100644 index 00000000..73ecafe6 --- /dev/null +++ b/rclcpp/actions/random_generator/action/Randomizer.action @@ -0,0 +1,8 @@ +uint16 number_of_values +float64 min_val +float64 max_val +--- +float64[] random_values +--- +uint16 number_of_random_values_calculated +float64 new_random_value \ No newline at end of file diff --git a/rclcpp/actions/random_generator/include/random_generator/visibility_control.h b/rclcpp/actions/random_generator/include/random_generator/visibility_control.h new file mode 100644 index 00000000..680d8cc5 --- /dev/null +++ b/rclcpp/actions/random_generator/include/random_generator/visibility_control.h @@ -0,0 +1,44 @@ +#ifndef RANDOM_GENERATOR__VISIBILITY_CONTROL_H_ +#define RANDOM_GENERATOR__VISIBILITY_CONTROL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RANDOM_GENERATOR_EXPORT __attribute__ ((dllexport)) + #define RANDOM_GENERATOR_IMPORT __attribute__ ((dllimport)) + #else + #define RANDOM_GENERATOR_EXPORT __declspec(dllexport) + #define RANDOM_GENERATOR_IMPORT __declspec(dllimport) + #endif + #ifdef RANDOM_GENERATOR_BUILDING_DLL + #define RANDOM_GENERATOR_PUBLIC RANDOM_GENERATOR_EXPORT + #else + #define RANDOM_GENERATOR_PUBLIC RANDOM_GENERATOR_IMPORT + #endif + #define RANDOM_GENERATOR_PUBLIC_TYPE RANDOM_GENERATOR_PUBLIC + #define RANDOM_GENERATOR_LOCAL +#else + #define RANDOM_GENERATOR_EXPORT __attribute__ ((visibility("default"))) + #define RANDOM_GENERATOR_IMPORT + #if __GNUC__ >= 4 + #define RANDOM_GENERATOR_PUBLIC __attribute__ ((visibility("default"))) + #define RANDOM_GENERATOR_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RANDOM_GENERATOR_PUBLIC + #define RANDOM_GENERATOR_LOCAL + #endif + #define RANDOM_GENERATOR_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // RANDOM_GENERATOR__VISIBILITY_CONTROL_H_ \ No newline at end of file diff --git a/rclcpp/actions/random_generator/package.xml b/rclcpp/actions/random_generator/package.xml new file mode 100644 index 00000000..67126497 --- /dev/null +++ b/rclcpp/actions/random_generator/package.xml @@ -0,0 +1,27 @@ + + + + random_generator + 0.0.0 + TODO: Package description + tirine + TODO: License declaration + + ament_cmake + rosidl_default_generators + + randomizer_action_interface + action_msgs + rclcpp + rclcpp_action + rclcpp_components + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rclcpp/actions/random_generator/src/randomizer_action_client.cpp b/rclcpp/actions/random_generator/src/randomizer_action_client.cpp new file mode 100644 index 00000000..3ad69ece --- /dev/null +++ b/rclcpp/actions/random_generator/src/randomizer_action_client.cpp @@ -0,0 +1,116 @@ +// Copyright 2016 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 + +#include "random_generator/action/randomizer.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace random_generator { +class RandomizerActionClient : public rclcpp::Node { + public: + using Randomizer = action::Randomizer; + using GoalHandleRandomizer = rclcpp_action::ClientGoalHandle; + + explicit RandomizerActionClient(const rclcpp::NodeOptions& options) + : Node("randomizer_action_client", options) { + this->client_ptr_ = rclcpp_action::create_client(this, "randomizer"); + + this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), + std::bind(&RandomizerActionClient::send_goal, this)); + } + + void send_goal() { + using namespace std::placeholders; + + this->timer_->cancel(); + + if (!this->client_ptr_->wait_for_action_server()) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = Randomizer::Goal(); + goal_msg.number_of_values = 10; + goal_msg.min_val = 0.5; + goal_msg.max_val = 9.5; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&RandomizerActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&RandomizerActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&RandomizerActionClient::result_callback, this, _1); + this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + } + + private: + rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::TimerBase::SharedPtr timer_; + + 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(GoalHandleRandomizer::SharedPtr, + const std::shared_ptr feedback) { + std::stringstream ss; + ss << std::setprecision(6); + ss << "Next random number: " << feedback->new_random_value + << ". \t Done: " << feedback->number_of_random_values_calculated << " values."; + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + } + + void result_callback(const GoalHandleRandomizer::WrappedResult& result_handle) { + switch (result_handle.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; + } + std::stringstream ss; + ss << "Result received: "; + for (auto number : result_handle.result->random_values) { + ss << number << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + rclcpp::shutdown(); + } +}; // class RandomizerActionClient + +} // namespace random_generator + +RCLCPP_COMPONENTS_REGISTER_NODE(random_generator::RandomizerActionClient) \ No newline at end of file diff --git a/rclcpp/actions/random_generator/src/randomizer_action_server.cpp b/rclcpp/actions/random_generator/src/randomizer_action_server.cpp new file mode 100644 index 00000000..896ac5bb --- /dev/null +++ b/rclcpp/actions/random_generator/src/randomizer_action_server.cpp @@ -0,0 +1,114 @@ +// Copyright 2016 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 "random_generator/action/randomizer.hpp" +#include "random_generator/visibility_control.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace random_generator { +class RandomizerActionServer : public rclcpp::Node { + public: + using Randomizer = action::Randomizer; + using GoalHandleRandomizer = rclcpp_action::ServerGoalHandle; + + RANDOM_GENERATOR_PUBLIC + explicit RandomizerActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) + : Node("randomizer_action_server", options) { + using namespace std::placeholders; + + this->action_server_ = rclcpp_action::create_server( + this, "randomizer", std::bind(&RandomizerActionServer::handle_goal, this, _1, _2), + std::bind(&RandomizerActionServer::handle_cancel, this, _1), + std::bind(&RandomizerActionServer::handle_accepted, this, _1)); + RCLCPP_INFO(this->get_logger(), "Started random generator action server"); + } + + private: + rclcpp_action::Server::SharedPtr action_server_; + + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) { + if (goal->min_val >= goal->max_val) { + RCLCPP_ERROR(this->get_logger(), + "Received goal request with %d values between %f and %f. Min value must be " + "smaller than the Max value.", + goal->number_of_values, goal->min_val, goal->max_val); + return rclcpp_action::GoalResponse::REJECT; + } + RCLCPP_INFO(this->get_logger(), "Received goal request with %d values between %f and %f", + goal->number_of_values, goal->min_val, goal->max_val); + (void)uuid; + 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 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(&RandomizerActionServer::execute, this, _1), goal_handle}.detach(); + } + + double random() { return ((long double)rand()) / ((long double)RAND_MAX); } + + void execute(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing goal"); + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + auto values = result->random_values; + for (feedback->number_of_random_values_calculated = 0; + feedback->number_of_random_values_calculated < goal->number_of_values && rclcpp::ok();) { + rclcpp::Rate loop_rate(5 * (random() + 0.1)); + loop_rate.sleep(); + // Check if there is a cancel request + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "Goal canceled"); + return; + } + // Get a new value + feedback->new_random_value = goal->min_val + (random()) * (goal->max_val - goal->min_val); + result->random_values.push_back(feedback->new_random_value); + feedback->number_of_random_values_calculated++; + // Publish feedback + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), "Publish feedback"); + } + + // Check if goal is done + if (rclcpp::ok()) { + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + } + } +}; // class RandomizerActionServer + +} // namespace random_generator + +RCLCPP_COMPONENTS_REGISTER_NODE(random_generator::RandomizerActionServer) \ No newline at end of file