diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..5632a7a --- /dev/null +++ b/.clang-format @@ -0,0 +1,32 @@ +# -*- yaml -*- + +# This file determines clang-format's style settings; for details, refer to +# http://clang.llvm.org/docs/ClangFormatStyleOptions.html + +# See https://google.github.io/styleguide/cppguide.html for more info +BasedOnStyle: Google + +Language: Cpp + +# https://clang.llvm.org/docs/ClangFormatStyleOptions.html#columnlimit +# Maximum line with is 120 characters (default: 80) +ColumnLimit : 120 + +# https://clang.llvm.org/docs/ClangFormatStyleOptions.html#pointeralignment +# Align pointer to the left: +# int* a; +DerivePointerAlignment: false +PointerAlignment: Left + +# https://clang.llvm.org/docs/ClangFormatStyleOptions.html#allowshortfunctionsonasingleline +# Compress functions onto a single line (when they fit) iff they are defined +# inline (inside a of class) or are empty. +AllowShortFunctionsOnASingleLine: Inline + +# https://clang.llvm.org/docs/ClangFormatStyleOptions.html#allowshortlambdasonasingleline +# Compress lambdas onto a single line iff they are empty. +AllowShortLambdasOnASingleLine: Empty + +# https://clang.llvm.org/docs/ClangFormatStyleOptions.html#includeblocks +# Alphabetically sort each block of includes separately, and do not re-group. +IncludeBlocks: Preserve \ No newline at end of file diff --git a/.github/workflows/cpplint.yml b/.github/workflows/cpplint.yml new file mode 100644 index 0000000..0f242c6 --- /dev/null +++ b/.github/workflows/cpplint.yml @@ -0,0 +1,10 @@ +name: cpplint +on: [push, pull_request] +jobs: + cpplint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v1 + - uses: actions/setup-python@v1 + - run: pip install cpplint + - run: cpplint --recursive . diff --git a/CMakeLists.txt b/CMakeLists.txt index a5a3429..9d1157c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -33,23 +33,23 @@ pkg_check_modules(readylogpp REQUIRED readylog++) pkg_check_modules(parsegologpp REQUIRED parsegolog++) set(gologpp_action_pkgs - turtlesim - gpp_action_examples_interface - nav2_msgs - webots_spot_msgs - spot_msgs + turtlesim + gpp_action_examples_interface + nav2_msgs + webots_spot_msgs + spot_msgs ) set(found_action_pkgs) foreach(pkg ${gologpp_action_pkgs}) - find_package(${pkg}) - if ("${${pkg}_FOUND}") - list(APPEND found_action_pkgs ${pkg}) - endif() + find_package(${pkg}) + if ("${${pkg}_FOUND}") + list(APPEND found_action_pkgs ${pkg}) + endif() endforeach() if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations -Wno-placement-new") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations -Wno-placement-new") endif() set(BUILD_TESTS OFF CACHE BOOL "Enable gologpp tests" FORCE) @@ -57,40 +57,45 @@ set(BUILD_TESTS OFF CACHE BOOL "Enable gologpp tests" FORCE) link_directories(${readylogpp_LIBRARY_DIRS} ${parsegologpp_LIBRARY_DIRS} ${ECLIPSE_LIBRARY_DIRS}) add_executable(gologpp_agent - src/agent_node.cpp - src/ros_backend.cpp - src/action_manager.cpp - src/exog_manager.cpp + src/agent_node.cpp + src/ros_backend.cpp + src/action_manager.cpp + src/exog_manager.cpp ) ament_target_dependencies(gologpp_agent rclcpp rclcpp_action rclcpp_components builtin_interfaces tf2 tf2_ros std_msgs std_srvs geometry_msgs ${found_action_pkgs}) target_link_libraries(gologpp_agent ${ECLIPSE_LIBRARIES}) target_include_directories(gologpp_agent PUBLIC - src/ - ${readylogpp_INCLUDE_DIRS} - ${parsegologpp_INCLUDE_DIRS} - ${ECLIPSE_INCLUDE_DIRS} + src/ + ${readylogpp_INCLUDE_DIRS} + ${parsegologpp_INCLUDE_DIRS} + ${ECLIPSE_INCLUDE_DIRS} ) install( - TARGETS gologpp_agent - DESTINATION lib/${PROJECT_NAME} + TARGETS gologpp_agent + DESTINATION lib/${PROJECT_NAME} ) foreach(pkg ${found_action_pkgs}) - target_sources(gologpp_agent PRIVATE "src/actions/gpp_${pkg}.cpp") - target_compile_definitions(gologpp_agent PUBLIC "-D${pkg}_FOUND") + target_sources(gologpp_agent PRIVATE "src/actions/gpp_${pkg}.cpp") + target_compile_definitions(gologpp_agent PUBLIC "-D${pkg}_FOUND") endforeach() target_link_libraries( - gologpp_agent - ${readylogpp_LIBRARIES} - ${parsegologpp_LIBRARIES} + gologpp_agent + ${readylogpp_LIBRARIES} + ${parsegologpp_LIBRARIES} +) +target_include_directories( + gologpp_agent + PRIVATE + include ) target_compile_features( - gologpp_agent - PUBLIC - c_std_99 - cxx_std_17 + gologpp_agent + PUBLIC + c_std_99 + cxx_std_17 ) ament_package() diff --git a/CPPLINT.cfg b/CPPLINT.cfg new file mode 100644 index 0000000..34fd832 --- /dev/null +++ b/CPPLINT.cfg @@ -0,0 +1,28 @@ +# Stop searching for additional config files. +set noparent + +# Disable a warning about C++ features that were not in the original +# C++11 specification (and so might not be well-supported). Our supported +# minimum platforms should be new enough that this warning is irrelevant. +filter=-build/c++11 + +# We do not care about the whitespace details of a TODO comment. It is not +# relevant for easy grepping, and the GSG does not specify any particular +# whitespace style. (We *do* care what the "TODO(username)" itself looks like +# because GSG forces a particular style there, but that formatting is covered +# by the readability/todo rule, which we leave enabled.) +filter=-whitespace/todo + +# Don't require a copyright in each file +filter=-legal/copyright + +# Must match clang-format +linelength=120 + +# Use `#pragma once`, not the `#ifndef FOO_H` guard. +# https://drake.mit.edu/styleguide/cppguide.html#The__pragma_once_Guard +filter=-build/header_guard +filter=+build/pragma_once + +# Trust clang-format to enforce the correct include order. +filter=-build/include_order \ No newline at end of file diff --git a/agents/spot.gpp b/agents/spot.gpp index ec71fed..ae14c88 100644 --- a/agents/spot.gpp +++ b/agents/spot.gpp @@ -1,13 +1,13 @@ action navigateTo(string target_frame, number x, number y, number yaw) { mapping: - "/trajectory" { - frame_id = target_frame, + "/trajectory" { + frame_id = target_frame, posX = x, posY = y, yaw = yaw - } + } } procedure main(){ - navigateTo("test_frame", 0.0, 0.0, 0.0); + navigateTo("test_frame", 0.0, 0.0, 0.0); } \ No newline at end of file diff --git a/agents/test_BAT.gpp b/agents/test_BAT.gpp index e36fd8c..5ae5285 100644 --- a/agents/test_BAT.gpp +++ b/agents/test_BAT.gpp @@ -1,94 +1,94 @@ action navigateToFrame(string target_frame) { mapping: - "trajectoryToFrame" { - frame_id = target_frame - } + "trajectoryToFrame" { + frame_id = target_frame + } } action playSound(string audio) { mapping: - "play_audio" { - audio_file = audio - } + "play_audio" { + audio_file = audio + } } // action clear_behavior_fault(bool data) { // mapping: -// "/clear_behavior_fault" { +// "/clear_behavior_fault" { // data = data -// } +// } // } action stop(number data) { mapping: - "/stop" {data = data} + "/stop" {data = data} } action increaseHeight() { mapping: - "body_pose_service" { - px = 0.0, - py = 0.0, - pz = 0.2, - ox = 0.0, - oy = 0.0, - oz = 0.0, - ow = 1.0 - } + "body_pose_service" { + px = 0.0, + py = 0.0, + pz = 0.2, + ox = 0.0, + oy = 0.0, + oz = 0.0, + ow = 1.0 + } } action reduceHeight() { mapping: - "body_pose_service" { - px = 0.0, - py = 0.0, - pz = -0.2, - ox = 0.0, - oy = 0.0, - oz = 0.0, - ow = 1.0 - } + "body_pose_service" { + px = 0.0, + py = 0.0, + pz = -0.2, + ox = 0.0, + oy = 0.0, + oz = 0.0, + ow = 1.0 + } } action normalPose() { mapping: - "body_pose_service" { - px = 0.0, - py = 0.0, - pz = 0.0, - ox = 0.0, - oy = 0.0, - oz = 0.0, - ow = 1.0 - } + "body_pose_service" { + px = 0.0, + py = 0.0, + pz = 0.0, + ox = 0.0, + oy = 0.0, + oz = 0.0, + ow = 1.0 + } } // action power_on() { // mapping: -// "/power_on" {} +// "/power_on" {} // } // action claim() { // mapping: -// "/claim" {} +// "/claim" {} // } // action sit() { // mapping: -// "/sit" {} +// "/sit" {} // } // action stand() { // mapping: -// "/stand" {} +// "/stand" {} // } procedure main(){ - start(playSound("Rickroll")); - navigateToFrame("frame1"); - increaseHeight(); - navigateToFrame("frame2"); - reduceHeight(); - navigateToFrame("frame3"); - normalPose(); - navigateToFrame("frame4"); + start(playSound("Rickroll")); + navigateToFrame("frame1"); + increaseHeight(); + navigateToFrame("frame2"); + reduceHeight(); + navigateToFrame("frame3"); + normalPose(); + navigateToFrame("frame4"); } \ No newline at end of file diff --git a/agents/turtlesim_example.gpp b/agents/turtlesim_example.gpp index fe6ee0b..27bcfcd 100644 --- a/agents/turtlesim_example.gpp +++ b/agents/turtlesim_example.gpp @@ -1,39 +1,39 @@ string fluent loc(string arg) { initially: - ("a") = ""; - ("b") = ""; - ("c") = ""; + ("a") = ""; + ("b") = ""; + ("c") = ""; } action print_string(string text) { mapping: - "/print_string" { - text = text - } + "/print_string" { + text = text + } } bool fluent exog_state(){ initially: - () = false; + () = false; } exog_action exog_trigger(bool data) { mapping: - "/exog_event" { - data = data - } + "/exog_event" { + data = data + } effect: - exog_state() = true; + exog_state() = true; } number fluent delta_result() { initially: - () = 0.0; + () = 0.0; } string fluent string_response() { initially: - () = ""; + () = ""; } string exog_function sense_result(string ros_action_name); @@ -42,33 +42,33 @@ number exog_function sense_number(string ros_action_name); action move_theta(number angle) { senses: - delta_result() = sense_number("/turtle1/rotate_absolute"); + delta_result() = sense_number("/turtle1/rotate_absolute"); mapping: - "/turtle1/rotate_absolute" { - theta = angle - } + "/turtle1/rotate_absolute" { + theta = angle + } } action spawn_turtle(number x, number y, number theta) { senses: - string_response() = sense_result("/spawn"); + string_response() = sense_result("/spawn"); mapping: - "/spawn" { - x = x, - y = y, - theta = theta - } + "/spawn" { + x = x, + y = y, + theta = theta + } } action move_circle(number seconds) { mapping: - "move_circle" { - seconds = seconds - } + "move_circle" { + seconds = seconds + } } procedure main(){ - move_circle(10.0); - test(exog_state()); + move_circle(10.0); + test(exog_state()); } diff --git a/cmake/FindEclipseClp.cmake b/cmake/FindEclipseClp.cmake index 26c9db7..4d5e7f5 100644 --- a/cmake/FindEclipseClp.cmake +++ b/cmake/FindEclipseClp.cmake @@ -1,9 +1,9 @@ function(get_eclipse_flag ECLIPSE_BIN FLAG RESULT) - execute_process(COMMAND - ${ECLIPSE_BIN} -e "get_flag(${FLAG}, D), printf(\"%p\", [D])" - OUTPUT_VARIABLE CMD_OUTPUT - ERROR_VARIABLE CMD_ERROR) - set(${RESULT} ${CMD_OUTPUT} PARENT_SCOPE) + execute_process(COMMAND + ${ECLIPSE_BIN} -e "get_flag(${FLAG}, D), printf(\"%p\", [D])" + OUTPUT_VARIABLE CMD_OUTPUT + ERROR_VARIABLE CMD_ERROR) + set(${RESULT} ${CMD_OUTPUT} PARENT_SCOPE) endfunction() execute_process(COMMAND /usr/bin/which eclipse-clp OUTPUT_VARIABLE ECLIPSE_BIN RESULT_VARIABLE WHICH_RESULT ERROR_QUIET) @@ -12,27 +12,27 @@ if (NOT ECLIPSE_BIN) endif() if (ECLIPSE_BIN) - string(STRIP ${ECLIPSE_BIN} ECLIPSE_BIN) - - if ((${WHICH_RESULT} EQUAL 0) AND (IS_ABSOLUTE ${ECLIPSE_BIN})) - get_eclipse_flag(${ECLIPSE_BIN} "version" ECLIPSE_VERSION) - get_eclipse_flag(${ECLIPSE_BIN} "installation_directory" ECLIPSE_DIR) - - if (IS_DIRECTORY ${ECLIPSE_DIR}) - message(STATUS "Found eclipse-clp ${ECLIPSE_VERSION} in ${ECLIPSE_DIR}") - set(ECLIPSE_FOUND true) - get_eclipse_flag(${ECLIPSE_BIN} "hostarch" ECLIPSE_HOSTARCH) - set(ECLIPSE_LIBRARIES "eclipse") - set(ECLIPSE_LIBDIR "${ECLIPSE_DIR}/lib/${ECLIPSE_HOSTARCH}") - set(ECLIPSE_INCDIR "${ECLIPSE_DIR}/include/${ECLIPSE_HOSTARCH}") - set(ECLIPSE_CFLAGS -I${ECLIPSE_INCDIR}) - set(ECLIPSE_LDFLAGS "-L${ECLIPSE_LIBDIR} -Wl,-R${ECLIPSE_LIBDIR} -leclipse") - set(ECLIPSE_LIBRARY_DIRS ${ECLIPSE_LIBDIR}) - foreach(DIR ${ECLIPSE_INCDIR} /usr/local/include/eclipse-clp /usr/include/eclipse-clp) - if (EXISTS ${DIR}/eclipse.h) - set(ECLIPSE_INCLUDE_DIRS ${ECLIPSE_INCLUDE_DIRS} ${DIR}) - endif() - endforeach() - endif() - endif() + string(STRIP ${ECLIPSE_BIN} ECLIPSE_BIN) + + if ((${WHICH_RESULT} EQUAL 0) AND (IS_ABSOLUTE ${ECLIPSE_BIN})) + get_eclipse_flag(${ECLIPSE_BIN} "version" ECLIPSE_VERSION) + get_eclipse_flag(${ECLIPSE_BIN} "installation_directory" ECLIPSE_DIR) + + if (IS_DIRECTORY ${ECLIPSE_DIR}) + message(STATUS "Found eclipse-clp ${ECLIPSE_VERSION} in ${ECLIPSE_DIR}") + set(ECLIPSE_FOUND true) + get_eclipse_flag(${ECLIPSE_BIN} "hostarch" ECLIPSE_HOSTARCH) + set(ECLIPSE_LIBRARIES "eclipse") + set(ECLIPSE_LIBDIR "${ECLIPSE_DIR}/lib/${ECLIPSE_HOSTARCH}") + set(ECLIPSE_INCDIR "${ECLIPSE_DIR}/include/${ECLIPSE_HOSTARCH}") + set(ECLIPSE_CFLAGS -I${ECLIPSE_INCDIR}) + set(ECLIPSE_LDFLAGS "-L${ECLIPSE_LIBDIR} -Wl,-R${ECLIPSE_LIBDIR} -leclipse") + set(ECLIPSE_LIBRARY_DIRS ${ECLIPSE_LIBDIR}) + foreach(DIR ${ECLIPSE_INCDIR} /usr/local/include/eclipse-clp /usr/include/eclipse-clp) + if (EXISTS ${DIR}/eclipse.h) + set(ECLIPSE_INCLUDE_DIRS ${ECLIPSE_INCLUDE_DIRS} ${DIR}) + endif() + endforeach() + endif() + endif() endif() \ No newline at end of file diff --git a/include/gologpp_agent/action_manager.h b/include/gologpp_agent/action_manager.h new file mode 100644 index 0000000..a35e8ff --- /dev/null +++ b/include/gologpp_agent/action_manager.h @@ -0,0 +1,209 @@ +#ifndef GOLOGPP_AGENT_ACTION_MANAGER_H_ +#define GOLOGPP_AGENT_ACTION_MANAGER_H_ + +// Remove spurious clang code model error +#ifdef Q_CREATOR_RUN +#undef __GCC_ASM_FLAG_OUTPUTS__ +#endif + +#include +#include + +#include +#include +#include + +#include "gologpp_agent/ros_backend.h" + +// Add ros2 action +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +using std::placeholders::_1; +namespace gpp = gologpp; + +class AbstractActionManager { + public: + explicit AbstractActionManager(const RosBackend& backend); + virtual ~AbstractActionManager() = default; + + /* Set current_activity_, call execute_current_activity() + * */ + void execute(gpp::shared_ptr); + void preempt(gpp::shared_ptr); + + virtual void execute_current_activity() = 0; + virtual void preempt_current_activity() = 0; + + gpp::optional result(); + void set_result(gpp::optional&& v); + + gpp::shared_ptr current_activity(); + + protected: + const RosBackend& backend_; + gpp::shared_ptr current_activity_; + gpp::optional result_; +}; + +template +class ActionManager : public AbstractActionManager { + public: + // Goal Handle and client + using GoalT = typename ActionT::Goal; + using ResultT = typename rclcpp_action::ClientGoalHandle; + using ClientT = typename rclcpp_action::Client::SharedPtr; + + ActionManager(const std::string&, const RosBackend& backend); + + void execute_current_activity() override; + void preempt_current_activity() override; + + // Specialized for every action type in e.g. pepper_actions.cpp + GoalT build_goal(const gpp::Activity&); + + // Result callback to transit action output to gpp agent + void result_callback(const typename ResultT::WrappedResult& result); + // ResultT::WrappedResult should be enough but rclp_action also before + // ::ResultT + gpp::optional to_golog_constant(typename ResultT::WrappedResult); + + private: + ClientT action_client_; + GoalT current_goal_; +}; + +template +class ServiceManager : public AbstractActionManager { + public: + using RequestT = typename ServiceT::Request::SharedPtr; + using ResponseT = typename rclcpp::Client::SharedFuture; + using Client = typename rclcpp::Client::SharedPtr; + + void execute_current_activity() override; + void preempt_current_activity() override; + + ServiceManager(const std::string&, const RosBackend& backend); + + RequestT build_request(const gpp::Activity&); + gpp::optional to_golog_constant(ResponseT); + + private: + Client service_client_; + RequestT current_request_; + ResponseT current_response_; +}; + +template +ServiceManager::ServiceManager(const std::string& topic_name, const RosBackend& backend) + : AbstractActionManager(backend) { + auto agent_node = Singleton::instance(); + service_client_ = agent_node->create_client(topic_name); +} + +template +ActionManager::ActionManager(const std::string& topic_name, const RosBackend& backend) + : AbstractActionManager(backend) { + auto agent_node = Singleton::instance(); + action_client_ = rclcpp_action::create_client(agent_node, topic_name); +} + +template +void ServiceManager::execute_current_activity() { + current_request_ = build_request(*current_activity_); + + while (!service_client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(LOGGER, "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_WARN_STREAM( + LOGGER, "Service " << std::string(current_activity_->mapped_name()) << " not available, waiting again..."); + } + + auto response_received_callback = [this](ResponseT future_response_) { + if (future_response_.valid()) { + current_activity_->update(gpp::Transition::Hook::FINISH); + set_result(to_golog_constant(future_response_)); + } else { + current_activity_->update(gpp::Transition::Hook::FAIL); + set_result(to_golog_constant(future_response_)); + RCLCPP_ERROR_STREAM(LOGGER, "Failed to call service " << std::string(current_activity_->mapped_name())); + } + }; + auto future_result = service_client_->async_send_request(current_request_, response_received_callback); +} + +template +void ServiceManager::preempt_current_activity() {} + +template +void ActionManager::preempt_current_activity() { + // Cancel goal + action_client_->async_cancel_all_goals(); + RCLCPP_ERROR(LOGGER, "Cancellation was requested."); +} + +template +void ActionManager::execute_current_activity() { + current_goal_ = build_goal(*current_activity_); + // ClientT is shared ptr otherwise useable for SendGoalOption + auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); + + send_goal_options.result_callback = std::bind(&ActionManager::result_callback, this, _1); + + while (!action_client_->wait_for_action_server(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(LOGGER, "Interrupted while waiting for the action. Exiting."); + return; + } + RCLCPP_WARN_STREAM(LOGGER, + "Action " << std::string(current_activity_->mapped_name()) << " not available, waiting again"); + } + + action_client_->async_send_goal(current_goal_, send_goal_options); +} + +template +void ActionManager::result_callback(const typename ResultT::WrappedResult& result) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + current_activity_->update(gpp::Transition::Hook::FINISH); + set_result(to_golog_constant(result)); + break; + case rclcpp_action::ResultCode::ABORTED: + current_activity_->update(gpp::Transition::Hook::FAIL); + set_result(to_golog_constant(result)); + break; + case rclcpp_action::ResultCode::CANCELED: + current_activity_->update(gpp::Transition::Hook::FAIL); + set_result(to_golog_constant(result)); + break; + default: + RCLCPP_ERROR(LOGGER, "Unknown result code"); + } +} + +template +gpp::optional ActionManager::to_golog_constant(typename ResultT::WrappedResult) { + return gpp::nullopt; +} + +template +gpp::optional ServiceManager::to_golog_constant(ServiceManager::ResponseT) { + return gpp::nullopt; +} + +template +void RosBackend::create_ActionManager(const std::string& topic_name) { + action_managers_.emplace(topic_name, + std::unique_ptr(new ActionManager(topic_name, *this))); +} + +template +void RosBackend::create_ServiceManager(const std::string& topic_name) { + action_managers_.emplace(topic_name, + std::unique_ptr(new ServiceManager(topic_name, *this))); +} + +#endif diff --git a/include/gologpp_agent/exog_manager.h b/include/gologpp_agent/exog_manager.h new file mode 100644 index 0000000..9291a86 --- /dev/null +++ b/include/gologpp_agent/exog_manager.h @@ -0,0 +1,88 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "gologpp_agent/ros_backend.h" + +namespace gpp = gologpp; + +using std::placeholders::_1; + +class RosBackend; + +class AbstractExogManager { + public: + explicit AbstractExogManager(const RosBackend& backend); + virtual ~AbstractExogManager() = default; + + protected: + const RosBackend& backend; +}; + +template +class ExogManager : public AbstractExogManager { + public: + ExogManager(const RosBackend& backend, const std::string& topic, int msgs_queue_size = 1000); + + typename rclcpp::Subscription::SharedPtr exog_subscriber_; + + void topic_cb(const typename ExogT::ConstPtr&); + void exog_event_to_queue(std::unordered_map>&& params_to_map); + std::unordered_map> params_to_map(const typename ExogT::ConstPtr& msg); + + private: + gpp::shared_ptr exog_; +}; + +template +ExogManager::ExogManager(const RosBackend& backend, const std::string& topic, int msgs_queue_size) + : AbstractExogManager(backend) { + auto agent_node = Singleton::instance(); + exog_subscriber_ = + agent_node->create_subscription(topic, 10, std::bind(&ExogManager::topic_cb, this, _1)); + + gpp::shared_ptr exog; + std::vector> global_vec = gpp::global_scope().globals(); + + for (std::vector>::iterator it = global_vec.begin(); it != global_vec.end(); ++it) { + if ((exog = std::dynamic_pointer_cast(*it))) { + if (exog->mapping().backend_name() == this->exog_subscriber_->get_topic_name()) { + exog_ = exog; + break; + } + } + } +} + +template +void ExogManager::topic_cb(const typename ExogT::ConstPtr& msg) { + if (backend.ctx_ready) { + // Topic msgs map to exog Event and pushed to Exog Queue + exog_event_to_queue(params_to_map(msg)); + } +} + +template +void ExogManager::exog_event_to_queue( + std::unordered_map>&& params_to_map) { + gpp::Binding param_binding; + + for (auto it = params_to_map.begin(); it != params_to_map.end(); it++) { + gpp::unique_ptr> param_ref{exog_->param_ref(it->first)}; + param_binding.bind(param_ref->target(), std::move(it->second)); + } + + gpp::shared_ptr ev{new gpp::ExogEvent(exog_, std::move(param_binding))}; + + gpp::ReadylogContext& ctx = gpp::ReadylogContext::instance(); + ctx.exog_queue_push(ev); +} + +template +void RosBackend::create_ExogManger(const std::string& topic) { + exog_managers_.push_back(std::unique_ptr(new ExogManager(*this, topic))); +} diff --git a/include/gologpp_agent/ros_backend.h b/include/gologpp_agent/ros_backend.h new file mode 100644 index 0000000..69fee4c --- /dev/null +++ b/include/gologpp_agent/ros_backend.h @@ -0,0 +1,104 @@ +#ifndef ROSBACKEND_H +#define ROSBACKEND_H + +// Remove spurious clang code model error +#ifdef Q_CREATOR_RUN +#undef __GCC_ASM_FLAG_OUTPUTS__ +#endif + +#define BOOST_BIND_GLOBAL_PLACEHOLDERS + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("gpp_ros_backend"); + +class AbstractActionManager; +class AbstractExogManager; + +class Singleton { + public: + Singleton(Singleton const&) = delete; + Singleton& operator=(Singleton const&) = delete; + // std::shared_ptr node = + // rclcpp::Node::make_shared("gologpp_agent"); + + static std::shared_ptr instance() { + static std::shared_ptr node = rclcpp::Node::make_shared("gologpp_agent"); + return node; + } + + private: + Singleton() {} +}; + +class RosBackend : public gologpp::PlatformBackend { + private: + using string = gologpp::string; + using Value = gologpp::Value; + using Type = gologpp::Type; + using Activity = gologpp::Activity; + using Clock = gologpp::Clock; + template + using shared_ptr = gologpp::shared_ptr; + + public: + RosBackend(); + ~RosBackend() override; + void execute_activity(shared_ptr a) override; + void preempt_activity(shared_ptr trans) override; + Clock::time_point time() const noexcept override; + + Value eval_exog_function(const Type& return_type, const string& backend_name, + const std::unordered_map& args) override; + + // std::mutex exog_mutex; + std::atomic ctx_ready; + + private: + void terminate_() override; + + std::vector built_interface_names; + + // May have an implementation if the corresponding package has been found + void define_turtlesim_actions(); + void define_gpp_action_examples_interface_actions(); + void define_webots_spot_msgs_actions(); + void define_nav2_msgs_actions(); + void define_spot_msgs_actions(); + + template + void create_ActionManager(const std::string& name); + + AbstractActionManager& get_ActionManager(shared_ptr); + + // create exogManager + template + void create_ExogManger(const std::string&); + + void spin_exog_thread(); + + template + void create_ServiceManager(const std::string&); + + std::vector> exog_managers_; + + std::unordered_map> action_managers_; +}; + +#endif // ROSBACKEND_H diff --git a/mapping_example.gpp b/mapping_example.gpp index b64d891..5f7d7d9 100644 --- a/mapping_example.gpp +++ b/mapping_example.gpp @@ -1,20 +1,20 @@ action movetoframe(string id) { mapping: - move_base { - x = 0, - y = 0, - yaw = 0, - frame_id = id - } + move_base { + x = 0, + y = 0, + yaw = 0, + frame_id = id + } } action go_to(number x, number y, number yaw) { mapping: - "move_base" { - x = x, - y = y, - yaw = yaw, - frame_id = "base_link" - } + "move_base" { + x = x, + y = y, + yaw = yaw, + frame_id = "base_link" + } } diff --git a/src/action_manager.cpp b/src/action_manager.cpp index 48cf67b..ce6f056 100644 --- a/src/action_manager.cpp +++ b/src/action_manager.cpp @@ -1,32 +1,27 @@ -#include "action_manager.h" +#include "gologpp_agent/action_manager.h" -AbstractActionManager::AbstractActionManager(RosBackend &backend) -: backend_(backend) -, result_() -{} +AbstractActionManager::AbstractActionManager(const RosBackend& backend) : backend_(backend), result_() {} - -void AbstractActionManager::execute(gpp::shared_ptra) { - // TODO: Set current_activity_, call execute_current_activity() - current_activity_ = a; - execute_current_activity(); +void AbstractActionManager::execute(gpp::shared_ptr a) { + current_activity_ = a; + execute_current_activity(); } -void AbstractActionManager::preempt(gpp::shared_ptra) { - current_activity_ = a; - preempt_current_activity(); +void AbstractActionManager::preempt(gpp::shared_ptr a) { + current_activity_ = a; + preempt_current_activity(); } -gpp::optional AbstractActionManager::result() -{ - auto rv { std::move(result_) }; - result_.reset(); - return rv; +gpp::optional AbstractActionManager::result() { + auto rv{std::move(result_)}; + result_.reset(); + return rv; } -void AbstractActionManager::set_result(gpp::optional &&v) -{ result_ = std::move(v); } - -gpp::shared_ptr AbstractActionManager::current_activity() -{ return current_activity_; } +void AbstractActionManager::set_result(gpp::optional&& v) { + result_ = std::move(v); +} +gpp::shared_ptr AbstractActionManager::current_activity() { + return current_activity_; +} diff --git a/src/action_manager.h b/src/action_manager.h deleted file mode 100644 index d7a6043..0000000 --- a/src/action_manager.h +++ /dev/null @@ -1,230 +0,0 @@ -#ifndef GOLOGPP_AGENT_ACTION_MANAGER_H_ -#define GOLOGPP_AGENT_ACTION_MANAGER_H_ - -// Remove spurious clang code model error -#ifdef Q_CREATOR_RUN -#undef __GCC_ASM_FLAG_OUTPUTS__ -#endif - -#include "ros_backend.h" - -#include -#include -#include - -// Add ros2 action -#include "rclcpp_action/rclcpp_action.hpp" -#include "rclcpp_components/register_node_macro.hpp" - -using namespace std::placeholders; -namespace gpp = gologpp; - - -class AbstractActionManager { -public: - AbstractActionManager(RosBackend &backend); - virtual ~AbstractActionManager() = default; - - /* Set current_activity_, call execute_current_activity() - * */ - void execute(gpp::shared_ptr); - void preempt(gpp::shared_ptr); - - virtual void execute_current_activity() = 0; - virtual void preempt_current_activity() = 0; - - gpp::optional result(); - void set_result(gpp::optional &&v); - - gpp::shared_ptr current_activity(); - -protected: - RosBackend &backend_; - gpp::shared_ptr current_activity_; - gpp::optional result_; -}; - - -template -class ActionManager : public AbstractActionManager { -public: - - // Goal Handle and client - using GoalT = typename ActionT::Goal; - using ResultT = typename rclcpp_action::ClientGoalHandle; - using ClientT = typename rclcpp_action::Client::SharedPtr; - - ActionManager(const std::string &, RosBackend &backend); - - virtual void execute_current_activity() override; - virtual void preempt_current_activity() override; - - // Specialized for every action type in e.g. pepper_actions.cpp - GoalT build_goal(const gpp::Activity &); - - // Result callback to transit action output to gpp agent - void result_callback(const typename ResultT::WrappedResult &result); - // ResultT::WrappedResult should be enough but rclp_action also before ::ResultT - gpp::optional to_golog_constant(typename ResultT::WrappedResult); - -private: - ClientT action_client_; - GoalT current_goal_; -}; - - -template -class ServiceManager : public AbstractActionManager { - // TODO -public: - using RequestT = typename ServiceT::Request::SharedPtr; - using ResponseT = typename rclcpp::Client::SharedFuture; - using Client = typename rclcpp::Client::SharedPtr; - - virtual void execute_current_activity() override; - virtual void preempt_current_activity() override; - - ServiceManager(const std::string &, RosBackend &backend); - - RequestT build_request(const gpp::Activity&); - gpp::optional to_golog_constant(ResponseT); - -private: - Client service_client_; - RequestT current_request_; - ResponseT current_response_; -}; - - -template -ServiceManager::ServiceManager(const std::string &topic_name, RosBackend &backend) -: AbstractActionManager (backend) -{ - auto agent_node = Singleton::instance(); - service_client_ = agent_node->create_client(topic_name); -} - - -template -ActionManager::ActionManager(const std::string &topic_name, RosBackend &backend) -: AbstractActionManager(backend) -{ - auto agent_node = Singleton::instance(); - action_client_ = rclcpp_action::create_client(agent_node, topic_name); - -} - - -template -void ServiceManager::execute_current_activity() { - current_request_ = build_request(*current_activity_); - - while (!service_client_->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(LOGGER, "Interrupted while waiting for the service. Exiting."); - return; - } - RCLCPP_WARN_STREAM(LOGGER, "Service " << std::string(current_activity_->mapped_name()) << " not available, waiting again..."); - } - - auto response_received_callback = [this](ResponseT future_response_) { - if (future_response_.valid()) { - current_activity_->update(gpp::Transition::Hook::FINISH); - set_result(to_golog_constant(future_response_)); - } else { - current_activity_->update(gpp::Transition::Hook::FAIL); - set_result(to_golog_constant(future_response_)); - RCLCPP_ERROR_STREAM(LOGGER, "Failed to call service " << std::string(current_activity_->mapped_name())); - } - }; - auto future_result = service_client_->async_send_request(current_request_, response_received_callback); -} - - -template -void ServiceManager::preempt_current_activity() {} - -template -void ActionManager::preempt_current_activity() -{ - //Cancel goal - action_client_->async_cancel_all_goals(); - RCLCPP_ERROR(LOGGER, "Cancellation was requested."); -} - - -template -void ActionManager::execute_current_activity() -{ - current_goal_ = build_goal(*current_activity_); - //ClientT is shared ptr otherwise useable for SendGoalOption - auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); - - send_goal_options.result_callback = - std::bind(&ActionManager::result_callback, this, _1); - - while (!action_client_->wait_for_action_server(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(LOGGER, "Interrupted while waiting for the action. Exiting."); - return; - } - RCLCPP_WARN_STREAM(LOGGER, "Action " << std::string(current_activity_->mapped_name()) << " not available, waiting again"); - } - - action_client_->async_send_goal(current_goal_, send_goal_options); -} - - -template -void ActionManager::result_callback(const typename ResultT::WrappedResult &result) { - switch(result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - current_activity_->update(gpp::Transition::Hook::FINISH); - set_result(to_golog_constant(result)); - break; - case rclcpp_action::ResultCode::ABORTED: - current_activity_->update(gpp::Transition::Hook::FAIL); - set_result(to_golog_constant(result)); - break; - case rclcpp_action::ResultCode::CANCELED: - current_activity_->update(gpp::Transition::Hook::FAIL); - set_result(to_golog_constant(result)); - break; - default: - RCLCPP_ERROR(LOGGER, "Unknown result code"); - } -} - - -template -gpp::optional ActionManager::to_golog_constant(typename ResultT::WrappedResult) -{ - return gpp::nullopt; -} - -template -gpp::optional ServiceManager::to_golog_constant(ServiceManager::ResponseT) -{ - return gpp::nullopt; -} - -template -void RosBackend::create_ActionManager(const std::string &topic_name) -{ - // TODO: Create ActionContainer and put in action_containers_ - action_managers_.emplace( - topic_name, - std::unique_ptr(new ActionManager(topic_name, *this)) - ); -} - -template -void RosBackend::create_ServiceManager(const std::string &topic_name) -{ - action_managers_.emplace( - topic_name, - std::unique_ptr(new ServiceManager(topic_name, *this)) - ); -} - -#endif diff --git a/src/actions/gologpp_darknet_action_msgs.cpp b/src/actions/gologpp_darknet_action_msgs.cpp deleted file mode 100644 index 6c575d3..0000000 --- a/src/actions/gologpp_darknet_action_msgs.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "action_manager.h" -#include "exog_manager.h" -#include "ros_backend.h" - -#include - -#include -#include - -#include - -namespace darknet = darknet_action_msgs; - -template<> -gpp::optional -ActionManager::to_golog_constant(ResultT result) { - return gpp::Value(gpp::get_type(), result->obj_pos); -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - darknet::obj_detectionGoal goal; - goal.to_detected_obj = std::string(a.mapped_arg_value("to_detected_obj")); - return goal; -} - -void RosBackend::define_darknet_actions() -{ - - create_ActionManager("/yolo_obj_detection_position_server"); - -} diff --git a/src/actions/gologpp_move_base_msgs.cpp b/src/actions/gologpp_move_base_msgs.cpp deleted file mode 100644 index 5418a26..0000000 --- a/src/actions/gologpp_move_base_msgs.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include "action_manager.h" -#include "exog_manager.h" -#include "ros_backend.h" - -#include - -#include - - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - move_base_msgs::MoveBaseGoal goal; - goal.target_pose.header.frame_id = std::string(a.mapped_arg_value("frame_id")); - goal.target_pose.header.stamp = ros::Time::now(); - goal.target_pose.pose.position.x = int(a.mapped_arg_value("x")); - goal.target_pose.pose.position.y = int(a.mapped_arg_value("y")); - goal.target_pose.pose.orientation.w = int(a.mapped_arg_value("w"));; - return goal; -} - -void RosBackend::define_move_base_actions() -{ create_ActionManager("move_base"); } diff --git a/src/actions/gologpp_naoqi_bridge_msgs.cpp b/src/actions/gologpp_naoqi_bridge_msgs.cpp deleted file mode 100644 index 6e9ee4a..0000000 --- a/src/actions/gologpp_naoqi_bridge_msgs.cpp +++ /dev/null @@ -1,36 +0,0 @@ -#include "exog_manager.h" -#include "ros_backend.h" -#include -#include - - - -template <> -std::unordered_map< std::string, gpp::unique_ptr > -ExogManager::params_to_map(const naoqi_bridge_msgs::Bumper::ConstPtr& msg) { - - gpp::unique_ptr param (new gpp::Value(gpp::get_type(), bool(msg->statePressed))); - std::unordered_map< std::string, gpp::unique_ptr > params_to_map; - params_to_map.insert({"pressed", std::move(param)}); - return params_to_map; -} - -template <> -std::unordered_map< std::string, gpp::unique_ptr > -ExogManager::params_to_map(const naoqi_bridge_msgs::HeadTouch::ConstPtr& msg) { - - gpp::unique_ptr param (new gpp::Value(gpp::get_type(), bool(msg->statePressed))); - std::unordered_map< std::string, gpp::unique_ptr > params_to_map; - params_to_map.insert({"pressed", std::move(param)}); - return params_to_map; -} - -void RosBackend::define_naoqi_bridge_actions() -{ - create_ExogManger( - "/pepper_robot/naoqi_driver/bumper" - ); - create_ExogManger( - "/pepper_robot/naoqi_driver/head_touch" - ); -} diff --git a/src/actions/gologpp_naoqi_wrapper_msgs.cpp b/src/actions/gologpp_naoqi_wrapper_msgs.cpp deleted file mode 100644 index 41d8bbd..0000000 --- a/src/actions/gologpp_naoqi_wrapper_msgs.cpp +++ /dev/null @@ -1,125 +0,0 @@ -#include "action_manager.h" -#include "exog_manager.h" -#include "ros_backend.h" - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace naoqi = naoqi_wrapper_msgs; - -template<> -gpp::optional -ActionManager::to_golog_constant(ResultT result){ - return gpp::Value(gpp::get_type(), result->outcome); -} - -template<> -gpp::optional -ActionManager::to_golog_constant(ResultT result) { - return gpp::Value(gpp::get_type(), result->command); -} - - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_dialogGoal goal; - goal.dialogTopicFile.data = std::string(a.mapped_arg_value("topic_file")); - return goal; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_sayGoal goal; - goal.message.data = std::string(a.mapped_arg_value("say_string")); - return goal; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_lookAtGoal goal; - goal.position.push_back(double(a.mapped_arg_value("x"))); - goal.position.push_back(double(a.mapped_arg_value("y"))); - goal.position.push_back(double(a.mapped_arg_value("z"))); - goal.frame = int(a.mapped_arg_value("frame")); - goal.fractionMaxSpeed = int(a.mapped_arg_value("fractionMaxSpeed")); - goal.useWholeBody = bool(a.mapped_arg_value("useWholeBody")); - return goal; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_openWebsiteGoal goal; - goal.url.data = std::string(a.mapped_arg_value("url")); - goal.waitForWebCommand = bool(a.mapped_arg_value("waitForWebCommand")); - return goal; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_animatedSayGoal goal; - goal.animatedMessage.data = std::string(a.mapped_arg_value("animatedMessage")); - return goal; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_animationGoal goal; - goal.animation.data = std::string(a.mapped_arg_value("animation")); - return goal; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_subscribeGoal goal; - goal.eventName.data = std::string(a.mapped_arg_value("eventName")); - return goal; -} - -template<> -ServiceManager::RequestT -ServiceManager::build_request(const gpp::Activity &a) -{ - naoqi::FaceTrackingRequest req; - req.enableFaceTracking = bool(a.mapped_arg_value("enable")); - return req; -} - -void RosBackend::define_naoqi_wrapper_actions() -{ - create_ActionManager("/naoqi_dialog_server"); - create_ActionManager("/naoqi_say_server/naoqi_say"); - create_ActionManager("/naoqi_lookAt_server/lookAt"); - create_ActionManager("/naoqi_openWebsite_server/openWebsite"); - create_ActionManager("/naoqi_animatedSay_server/animatedSay"); - create_ActionManager("/naoqi_animation_server/naoqi_animation"); - create_ActionManager("/naoqi_subscribe_server/subscribe"); - create_ServiceManager("/face_tracking"); -} - diff --git a/src/actions/gologpp_opencv_apps_msgs.cpp b/src/actions/gologpp_opencv_apps_msgs.cpp deleted file mode 100644 index 2020939..0000000 --- a/src/actions/gologpp_opencv_apps_msgs.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include "exog_manager.h" -#include "action_manager.h" -#include "ros_backend.h" - -#include - -#include -#include - -template <> -std::unordered_map< std::string, gpp::unique_ptr > -ExogManager::params_to_map(const opencv_apps::FaceArrayStamped::ConstPtr& msg) { - - gpp::unique_ptr param_label (new gpp::Value(gpp::get_type(), std::string(msg->faces[0].label))); - gpp::unique_ptr param_confidence (new gpp::Value(gpp::get_type(), int(msg->faces[0].confidence))); - std::unordered_map< std::string, gpp::unique_ptr > params_to_map; - params_to_map.insert({"name", std::move(param_label)}); - params_to_map.insert({"confidence", std::move(param_confidence)}); - return params_to_map; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - opencv_apps_action_msgs::LearnFaceGoal goal; - goal.name = std::string(a.mapped_arg_value("learn_name")); - return goal; -} - -void RosBackend::define_opencv_apps_actions() -{ - create_ActionManager( - "/face_recognition_trainer_actionserver" - ); - create_ExogManger( - "/face_recognition/output" - ); -} - diff --git a/src/actions/gologpp_turtle_actionlib.cpp b/src/actions/gologpp_turtle_actionlib.cpp deleted file mode 100644 index 8a69cd1..0000000 --- a/src/actions/gologpp_turtle_actionlib.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include "action_manager.h" -#include "exog_manager.h" -#include "ros_backend.h" - -#include - -#include - - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - turtle_actionlib::ShapeGoal goal; - goal.edges = a.mapped_arg_value("edges").numeric_convert(); - goal.radius = a.mapped_arg_value("radius").numeric_convert(); - return goal; -} - - -void RosBackend::define_turtle_actions() -{ create_ActionManager("turtle_shape"); } diff --git a/src/actions/gpp_gpp_action_examples_interface.cpp b/src/actions/gpp_gpp_action_examples_interface.cpp index 9d3bb35..2ab78cc 100644 --- a/src/actions/gpp_gpp_action_examples_interface.cpp +++ b/src/actions/gpp_gpp_action_examples_interface.cpp @@ -1,80 +1,72 @@ -#include "action_manager.h" -#include "exog_manager.h" -#include "ros_backend.h" - #include -#include "gpp_action_examples_interface/srv/spot_body_pose.hpp" -#include "gpp_action_examples_interface/srv/print.hpp" -#include "gpp_action_examples_interface/action/trajectory_to_frame.hpp" -#include "gpp_action_examples_interface/action/play_audio.hpp" +#include "gologpp_agent/action_manager.h" +#include "gologpp_agent/exog_manager.h" +#include "gologpp_agent/ros_backend.h" #include "gpp_action_examples_interface/action/durative.hpp" +#include "gpp_action_examples_interface/action/play_audio.hpp" +#include "gpp_action_examples_interface/action/trajectory_to_frame.hpp" +#include "gpp_action_examples_interface/srv/print.hpp" +#include "gpp_action_examples_interface/srv/spot_body_pose.hpp" -template<> +template <> ServiceManager::RequestT -ServiceManager::build_request(const gpp::Activity &a) -{ - auto request = std::make_shared(); - request->request_print = std::string(a.mapped_arg_value("text")); - return request; +ServiceManager::build_request(const gpp::Activity& a) { + auto request = std::make_shared(); + request->request_print = std::string(a.mapped_arg_value("text")); + return request; } -template<> -gpp::optional -ServiceManager::to_golog_constant(ResponseT result){ - return gpp::Value(gpp::get_type(), result.get()->response_print); +template <> +gpp::optional ServiceManager::to_golog_constant( + ResponseT result) { + return gpp::Value(gpp::get_type(), result.get()->response_print); } -template<> +template <> ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - auto goal = gpp_action_examples_interface::action::TrajectoryToFrame::Goal(); - goal.frame_id = std::string(a.mapped_arg_value("frame_id")); - return goal; +ActionManager::build_goal(const gpp::Activity& a) { + auto goal = gpp_action_examples_interface::action::TrajectoryToFrame::Goal(); + goal.frame_id = std::string(a.mapped_arg_value("frame_id")); + return goal; } -template<> +template <> ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - auto goal = gpp_action_examples_interface::action::PlayAudio::Goal(); - goal.audio_file = std::string(a.mapped_arg_value("audio_file")); - return goal; +ActionManager::build_goal(const gpp::Activity& a) { + auto goal = gpp_action_examples_interface::action::PlayAudio::Goal(); + goal.audio_file = std::string(a.mapped_arg_value("audio_file")); + return goal; } -template<> +template <> ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - auto goal = gpp_action_examples_interface::action::Durative::Goal(); - goal.duration.sec = a.mapped_arg_value("seconds").numeric_convert(); - return goal; +ActionManager::build_goal(const gpp::Activity& a) { + auto goal = gpp_action_examples_interface::action::Durative::Goal(); + goal.duration.sec = a.mapped_arg_value("seconds").numeric_convert(); + return goal; } - -template<> +template <> ServiceManager::RequestT -ServiceManager::build_request(const gpp::Activity &a) -{ - auto request = std::make_shared(); - request->pose.position.x = a.mapped_arg_value("px").numeric_convert(); - request->pose.position.y = a.mapped_arg_value("py").numeric_convert(); - request->pose.position.z = a.mapped_arg_value("pz").numeric_convert(); - request->pose.orientation.x = a.mapped_arg_value("ox").numeric_convert(); - request->pose.orientation.y = a.mapped_arg_value("oy").numeric_convert(); - request->pose.orientation.z = a.mapped_arg_value("oz").numeric_convert(); - request->pose.orientation.w = a.mapped_arg_value("ow").numeric_convert(); - return request; +ServiceManager::build_request(const gpp::Activity& a) { + auto request = std::make_shared(); + request->pose.position.x = a.mapped_arg_value("px").numeric_convert(); + request->pose.position.y = a.mapped_arg_value("py").numeric_convert(); + request->pose.position.z = a.mapped_arg_value("pz").numeric_convert(); + request->pose.orientation.x = a.mapped_arg_value("ox").numeric_convert(); + request->pose.orientation.y = a.mapped_arg_value("oy").numeric_convert(); + request->pose.orientation.z = a.mapped_arg_value("oz").numeric_convert(); + request->pose.orientation.w = a.mapped_arg_value("ow").numeric_convert(); + return request; } -void RosBackend::define_gpp_action_examples_interface_actions() -{ - built_interface_names.push_back("gpp_action_examples"); +void RosBackend::define_gpp_action_examples_interface_actions() { + built_interface_names.push_back("gpp_action_examples"); - create_ActionManager("move_circle"); - create_ActionManager("trajectoryToFrame"); - create_ActionManager("play_audio"); - create_ServiceManager("/print_string"); - create_ServiceManager("body_pose_service"); -} \ No newline at end of file + create_ActionManager("move_circle"); + create_ActionManager("trajectoryToFrame"); + create_ActionManager("play_audio"); + create_ServiceManager("/print_string"); + create_ServiceManager("body_pose_service"); +} diff --git a/src/actions/gpp_nav2_msgs.cpp b/src/actions/gpp_nav2_msgs.cpp index f2ad274..bb84fba 100644 --- a/src/actions/gpp_nav2_msgs.cpp +++ b/src/actions/gpp_nav2_msgs.cpp @@ -1,34 +1,31 @@ -#include "action_manager.h" -#include "ros_backend.h" - #include +#include "gologpp_agent/action_manager.h" +#include "gologpp_agent/ros_backend.h" #include "nav2_msgs/action/navigate_to_pose.hpp" -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - auto goal = nav2_msgs::action::NavigateToPose::Goal(); - auto agent_node = Singleton::instance(); - goal.pose.header.stamp = rclcpp::Clock().now(); - goal.pose.header.frame_id = "map"; +template <> +ActionManager::GoalT ActionManager::build_goal( + const gpp::Activity& a) { + auto goal = nav2_msgs::action::NavigateToPose::Goal(); + auto agent_node = Singleton::instance(); + goal.pose.header.stamp = rclcpp::Clock().now(); + goal.pose.header.frame_id = "map"; - goal.pose.pose.position.x = a.mapped_arg_value("px").numeric_convert(); - goal.pose.pose.position.y = a.mapped_arg_value("py").numeric_convert(); - goal.pose.pose.position.z = a.mapped_arg_value("pz").numeric_convert(); + goal.pose.pose.position.x = a.mapped_arg_value("px").numeric_convert(); + goal.pose.pose.position.y = a.mapped_arg_value("py").numeric_convert(); + goal.pose.pose.position.z = a.mapped_arg_value("pz").numeric_convert(); - goal.pose.pose.orientation.x = a.mapped_arg_value("ox").numeric_convert(); - goal.pose.pose.orientation.y = a.mapped_arg_value("oy").numeric_convert(); - goal.pose.pose.orientation.z = a.mapped_arg_value("oz").numeric_convert(); - goal.pose.pose.orientation.w = a.mapped_arg_value("ow").numeric_convert(); + goal.pose.pose.orientation.x = a.mapped_arg_value("ox").numeric_convert(); + goal.pose.pose.orientation.y = a.mapped_arg_value("oy").numeric_convert(); + goal.pose.pose.orientation.z = a.mapped_arg_value("oz").numeric_convert(); + goal.pose.pose.orientation.w = a.mapped_arg_value("ow").numeric_convert(); - return goal; + return goal; } -void RosBackend::define_nav2_msgs_actions() -{ - built_interface_names.push_back("nav2_msgs"); +void RosBackend::define_nav2_msgs_actions() { + built_interface_names.push_back("nav2_msgs"); - create_ActionManager("/navigate_to_pose"); -} \ No newline at end of file + create_ActionManager("/navigate_to_pose"); +} diff --git a/src/actions/gpp_spot_msgs.cpp b/src/actions/gpp_spot_msgs.cpp index b1163d0..66d34fc 100644 --- a/src/actions/gpp_spot_msgs.cpp +++ b/src/actions/gpp_spot_msgs.cpp @@ -1,133 +1,117 @@ -#include "action_manager.h" -#include "exog_manager.h" -#include "ros_backend.h" - #include +#include "builtin_interfaces/msg/duration.h" +#include "builtin_interfaces/msg/time.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "gologpp_agent/action_manager.h" +#include "gologpp_agent/exog_manager.h" +#include "gologpp_agent/ros_backend.h" +#include "spot_msgs/action/trajectory.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "std_srvs/srv/trigger.hpp" #include "tf2/LinearMath/Quaternion.h" #include "tf2/exceptions.h" -#include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" -#include "spot_msgs/action/trajectory.hpp" +template <> +ActionManager::GoalT ActionManager::build_goal( + const gpp::Activity& a) { + auto agent_node = Singleton::instance(); + auto goal = spot_msgs::action::Trajectory::Goal(); + auto target_pose = geometry_msgs::msg::PoseStamped(); + auto frame = std::string(a.mapped_arg_value("frame_id")); + builtin_interfaces::msg::Time time = agent_node->get_clock()->now(); + tf2::Quaternion q; + float yaw = 0; + auto duration = builtin_interfaces::msg::Duration(); + + target_pose.header.stamp = time; + duration.sec = 10; + duration.nanosec = 0.0; + + goal.duration = duration; + goal.precise_positioning = false; // bool(a.mapped_arg_value("precise_positioning")); + + if (frame == "body") { + target_pose.header.frame_id = std::string(a.mapped_arg_value("frame_id")); + target_pose.pose.position.x = a.mapped_arg_value("posX").numeric_convert(); + target_pose.pose.position.y = a.mapped_arg_value("posY").numeric_convert(); + target_pose.pose.position.z = 0.0; // is not used + + yaw = a.mapped_arg_value("yaw").numeric_convert(); + + q.setRPY(0.0, 0.0, yaw); + + target_pose.pose.orientation.x = q.x(); + target_pose.pose.orientation.y = q.y(); + target_pose.pose.orientation.z = q.z(); + target_pose.pose.orientation.w = q.w(); + goal.target_pose = target_pose; + + return goal; + } else { + std::unique_ptr tf_buffer_; + tf_buffer_ = std::make_unique(agent_node->get_clock()); + geometry_msgs::msg::TransformStamped t; + // Look up for the transformation between target_frame and body frames + // and send goal relativ from body + try { + t = tf_buffer_->lookupTransform("body", frame, tf2::TimePointZero); + } catch (const tf2::TransformException& ex) { + RCLCPP_INFO(LOGGER, "Could not transform %s to %s: %s", frame.c_str(), "body", ex.what()); + target_pose.header.frame_id = "ABORT_ACTION"; + return goal; + } catch (tf2::ConnectivityException& e) { + RCLCPP_WARN(LOGGER, e.what()); + target_pose.header.frame_id = "ABORT_ACTION"; + return goal; + } catch (tf2::ExtrapolationException& e) { + RCLCPP_WARN(LOGGER, e.what()); + target_pose.header.frame_id = "ABORT_ACTION"; + return goal; + } catch (tf2::LookupException& e) { + RCLCPP_WARN(LOGGER, e.what()); + target_pose.header.frame_id = "ABORT_ACTION"; + return goal; + } -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "builtin_interfaces/msg/duration.h" -#include "builtin_interfaces/msg/time.hpp" + target_pose.header.frame_id = "body"; + target_pose.pose.position.x = t.transform.translation.x; + target_pose.pose.position.y = t.transform.translation.y; + target_pose.pose.position.z = 0.0; // is not used -#include "std_msgs/msg/bool.hpp" -#include "std_srvs/srv/trigger.hpp" -#include "std_srvs/srv/set_bool.hpp" + target_pose.pose.orientation.x = 0.0; + target_pose.pose.orientation.y = 0.0; + target_pose.pose.orientation.z = 0.0; + target_pose.pose.orientation.w = t.transform.rotation.w; -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - auto agent_node = Singleton::instance(); - auto goal = spot_msgs::action::Trajectory::Goal(); - auto target_pose = geometry_msgs::msg::PoseStamped(); - auto frame = std::string(a.mapped_arg_value("frame_id")); - builtin_interfaces::msg::Time time = agent_node->get_clock()->now(); - tf2::Quaternion q; - float yaw = 0; - auto duration = builtin_interfaces::msg::Duration(); - - target_pose.header.stamp = time; - duration.sec = 10; - duration.nanosec = 0.0; - - goal.duration = duration; - goal.precise_positioning = false;//bool(a.mapped_arg_value("precise_positioning")); - - if (frame == "body"){ - target_pose.header.frame_id = std::string(a.mapped_arg_value("frame_id")); - target_pose.pose.position.x = a.mapped_arg_value("posX").numeric_convert(); - target_pose.pose.position.y = a.mapped_arg_value("posY").numeric_convert(); - target_pose.pose.position.z = 0.0; // is not used - - yaw = a.mapped_arg_value("yaw").numeric_convert(); - - q.setRPY(0.0,0.0,yaw); - - target_pose.pose.orientation.x = q.x(); - target_pose.pose.orientation.y = q.y(); - target_pose.pose.orientation.z = q.z(); - target_pose.pose.orientation.w = q.w(); - goal.target_pose = target_pose; - - return goal; - - }else{ - std::unique_ptr tf_buffer_; - tf_buffer_ = std::make_unique(agent_node->get_clock()); - geometry_msgs::msg::TransformStamped t; - // Look up for the transformation between target_frame and body frames - // and send goal relativ from body - try { - t = tf_buffer_->lookupTransform( - "body", frame, - tf2::TimePointZero); - } catch (const tf2::TransformException & ex) { - RCLCPP_INFO( - LOGGER, "Could not transform %s to %s: %s", - frame.c_str(), "body", ex.what()); - target_pose.header.frame_id = "ABORT_ACTION"; - return goal; - } - catch (tf2::ConnectivityException& e) { - RCLCPP_WARN(LOGGER, e.what()); - target_pose.header.frame_id = "ABORT_ACTION"; - return goal; - } - catch (tf2::ExtrapolationException& e) { - RCLCPP_WARN(LOGGER, e.what()); - target_pose.header.frame_id = "ABORT_ACTION"; - return goal; - } - catch (tf2::LookupException& e) { - RCLCPP_WARN(LOGGER, e.what()); - target_pose.header.frame_id = "ABORT_ACTION"; - return goal; - } - - target_pose.header.frame_id = "body"; - target_pose.pose.position.x = t.transform.translation.x; - target_pose.pose.position.y = t.transform.translation.y; - target_pose.pose.position.z = 0.0; // is not used - - target_pose.pose.orientation.x = 0.0; - target_pose.pose.orientation.y = 0.0; - target_pose.pose.orientation.z = 0.0; - target_pose.pose.orientation.w = t.transform.rotation.w; - - goal.target_pose = target_pose; - - return goal; - } + goal.target_pose = target_pose; + + return goal; + } } -template<> -ServiceManager::RequestT -ServiceManager::build_request(const gpp::Activity &a) -{ +template <> +ServiceManager::RequestT ServiceManager::build_request( + const gpp::Activity& a) { auto request = std::make_shared(); return request; } -template<> -ServiceManager::RequestT -ServiceManager::build_request(const gpp::Activity &a) -{ +template <> +ServiceManager::RequestT ServiceManager::build_request( + const gpp::Activity& a) { auto request = std::make_shared(); - request->data = bool(a.mapped_arg_value("data")); + request->data = static_cast(a.mapped_arg_value("data")); return request; } -void RosBackend::define_spot_msgs_actions() -{ - built_interface_names.push_back("spot_msgs"); +void RosBackend::define_spot_msgs_actions() { + built_interface_names.push_back("spot_msgs"); create_ActionManager("/trajectory"); @@ -147,4 +131,4 @@ void RosBackend::define_spot_msgs_actions() create_ExogManger("/exog_event_next_TF"); create_ExogManger("/exog_event_next_Lap"); -} \ No newline at end of file +} diff --git a/src/actions/gpp_turtlesim.cpp b/src/actions/gpp_turtlesim.cpp index d5f89a5..564285d 100644 --- a/src/actions/gpp_turtlesim.cpp +++ b/src/actions/gpp_turtlesim.cpp @@ -1,63 +1,57 @@ -#include "action_manager.h" -#include "exog_manager.h" -#include "ros_backend.h" - #include +#include "gologpp_agent/action_manager.h" +#include "gologpp_agent/exog_manager.h" +#include "gologpp_agent/ros_backend.h" +#include "std_msgs/msg/bool.hpp" #include "turtlesim/action/rotate_absolute.hpp" #include "turtlesim/srv/spawn.hpp" -#include "std_msgs/msg/bool.hpp" -template<> -std::unordered_map< std::string, gpp::unique_ptr > -ExogManager::params_to_map(const std_msgs::msg::Bool::ConstPtr& msg) { - - gpp::unique_ptr param (new gpp::Value(gpp::get_type(), bool(msg->data))); - std::unordered_map< std::string, gpp::unique_ptr > params_to_map; - params_to_map.insert({"data", std::move(param)}); - return params_to_map; +template <> +std::unordered_map> ExogManager::params_to_map( + const std_msgs::msg::Bool::ConstPtr& msg) { + gpp::unique_ptr param(new gpp::Value(gpp::get_type(), static_cast(msg->data))); + std::unordered_map> params_to_map; + params_to_map.insert({"data", std::move(param)}); + return params_to_map; } -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - auto goal = turtlesim::action::RotateAbsolute::Goal(); - goal.theta = a.mapped_arg_value("theta").numeric_convert(); - return goal; +template <> +ActionManager::GoalT ActionManager::build_goal( + const gpp::Activity& a) { + auto goal = turtlesim::action::RotateAbsolute::Goal(); + goal.theta = a.mapped_arg_value("theta").numeric_convert(); + return goal; } -template<> -gpp::optional -ActionManager::to_golog_constant(ResultT::WrappedResult result){ - std::cout<<"result"<(), result.result->delta); +template <> +gpp::optional ActionManager::to_golog_constant( + ResultT::WrappedResult result) { + std::cout << "result" << std::endl; + return gpp::Value(gpp::get_type(), result.result->delta); } -template<> -ServiceManager::RequestT -ServiceManager::build_request(const gpp::Activity &a) -{ - auto request = std::make_shared(); - request->x = a.mapped_arg_value("x").numeric_convert(); - request->y = a.mapped_arg_value("y").numeric_convert(); - request->theta = a.mapped_arg_value("theta").numeric_convert(); - return request; +template <> +ServiceManager::RequestT ServiceManager::build_request( + const gpp::Activity& a) { + auto request = std::make_shared(); + request->x = a.mapped_arg_value("x").numeric_convert(); + request->y = a.mapped_arg_value("y").numeric_convert(); + request->theta = a.mapped_arg_value("theta").numeric_convert(); + return request; } -template<> -gpp::optional -ServiceManager::to_golog_constant(ResponseT result){ - return gpp::Value(gpp::get_type(), result.get()->name); +template <> +gpp::optional ServiceManager::to_golog_constant(ResponseT result) { + return gpp::Value(gpp::get_type(), result.get()->name); } -void RosBackend::define_turtlesim_actions() -{ - built_interface_names.push_back("turtlesim"); +void RosBackend::define_turtlesim_actions() { + built_interface_names.push_back("turtlesim"); - create_ActionManager("/turtle1/rotate_absolute"); + create_ActionManager("/turtle1/rotate_absolute"); - create_ServiceManager("/spawn"); + create_ServiceManager("/spawn"); - create_ExogManger("/exog_event"); -} \ No newline at end of file + create_ExogManger("/exog_event"); +} diff --git a/src/actions/gpp_webots_spot_msgs.cpp b/src/actions/gpp_webots_spot_msgs.cpp index 72207db..bce3374 100644 --- a/src/actions/gpp_webots_spot_msgs.cpp +++ b/src/actions/gpp_webots_spot_msgs.cpp @@ -1,79 +1,66 @@ -#include "action_manager.h" -#include "exog_manager.h" -#include "ros_backend.h" - #include -#include "webots_spot_msgs/srv/block_pose.hpp" +#include "gologpp_agent/action_manager.h" +#include "gologpp_agent/exog_manager.h" +#include "gologpp_agent/ros_backend.h" +#include "webots_spot_msgs/action/peak_and_detect_object.hpp" #include "webots_spot_msgs/action/stack.hpp" -#include "webots_spot_msgs/srv/spot_motion.hpp" +#include "webots_spot_msgs/srv/block_pose.hpp" #include "webots_spot_msgs/srv/spot_height.hpp" -#include "webots_spot_msgs/action/peak_and_detect_object.hpp" +#include "webots_spot_msgs/srv/spot_motion.hpp" -template<> -gpp::optional -ServiceManager::to_golog_constant(ResponseT result){ - return gpp::Value(gpp::get_type(), result.get()->location); +template <> +gpp::optional ServiceManager::to_golog_constant(ResponseT result) { + return gpp::Value(gpp::get_type(), result.get()->location); } -template<> +template <> ServiceManager::RequestT -ServiceManager::build_request(const gpp::Activity &a) -{ - auto request = std::make_shared(); - request->block = std::string(a.mapped_arg_value("block")); - return request; +ServiceManager::build_request(const gpp::Activity& a) { + auto request = std::make_shared(); + request->block = std::string(a.mapped_arg_value("block")); + return request; } - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - auto goal = webots_spot_msgs::action::Stack::Goal(); - goal.block = std::string(a.mapped_arg_value("block")); - goal.location = std::string(a.mapped_arg_value("location")); - return goal; +template <> +ActionManager::GoalT ActionManager::build_goal( + const gpp::Activity& a) { + auto goal = webots_spot_msgs::action::Stack::Goal(); + goal.block = std::string(a.mapped_arg_value("block")); + goal.location = std::string(a.mapped_arg_value("location")); + return goal; } - -template<> +template <> ServiceManager::RequestT -ServiceManager::build_request(const gpp::Activity &a) -{ - auto request = std::make_shared(); - request->override = std::string(a.mapped_arg_value("override")) == "true" ? true:false; - return request; +ServiceManager::build_request(const gpp::Activity& a) { + auto request = std::make_shared(); + request->override = std::string(a.mapped_arg_value("override")) == "true" ? true : false; + return request; } - -template<> +template <> ServiceManager::RequestT -ServiceManager::build_request(const gpp::Activity &a) -{ - auto request = std::make_shared(); - request->height = a.mapped_arg_value("height").numeric_convert(); - return request; +ServiceManager::build_request(const gpp::Activity& a) { + auto request = std::make_shared(); + request->height = a.mapped_arg_value("height").numeric_convert(); + return request; } - -template<> +template <> ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - auto goal = webots_spot_msgs::action::PeakAndDetectObject::Goal(); - goal.image = std::string(a.mapped_arg_value("image")); - return goal; +ActionManager::build_goal(const gpp::Activity& a) { + auto goal = webots_spot_msgs::action::PeakAndDetectObject::Goal(); + goal.image = std::string(a.mapped_arg_value("image")); + return goal; } +void RosBackend::define_webots_spot_msgs_actions() { + built_interface_names.push_back("webots_spot_msgs"); -void RosBackend::define_webots_spot_msgs_actions() -{ - built_interface_names.push_back("webots_spot_msgs"); - - create_ServiceManager("/get_block_pose"); - create_ActionManager("/stack"); - create_ServiceManager("/Spot/lie_down"); - create_ServiceManager("/Spot/set_height"); - create_ActionManager("/detect_object_in_box"); + create_ServiceManager("/get_block_pose"); + create_ActionManager("/stack"); + create_ServiceManager("/Spot/lie_down"); + create_ServiceManager("/Spot/set_height"); + create_ActionManager("/detect_object_in_box"); } diff --git a/src/actions/pepper_actions.cpp b/src/actions/pepper_actions.cpp deleted file mode 100644 index e54e851..0000000 --- a/src/actions/pepper_actions.cpp +++ /dev/null @@ -1,201 +0,0 @@ -#include "action_manager.h" -#include "exog_manager.h" -#include "ros_backend.h" - -#include - -#include -#include - -#ifdef move_base_msgs_FOUND -#include -#endif - -#ifdef darknet_action_msgs_FOUND -#include -#endif - -#ifdef naoqi_bridge_msgs_FOUND -#include -#endif - -#ifdef naoqi_wrapper_msgs_FOUND -#include -#include -#include -#include -#include -#include -#include -#include - -namespace naoqi = naoqi_wrapper_msgs; -#endif - -#ifdef darknet_action_msgs_FOUND -namespace darknet = darknet_action_msgs; - - -template<> -gpp::Value * -ActionManager::to_golog_constant(ResultT result) { - return new gpp::Value(gpp::NumberType::name(),result->obj_pos); -} -#endif // darknet_action_msgs_FOUND - -#ifdef naoqi_wrapper_msgs_FOUND -template<> -gpp::Value * -ActionManager::to_golog_constant(ResultT result){ - return new gpp::Value(gpp::StringType::name(), result->outcome); -} - -template<> -gpp::Value * -ActionManager::to_golog_constant(ResultT result) { - return new gpp::Value(gpp::StringType::name(), result->command); -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_dialogGoal goal; - goal.dialogTopicFile.data = std::string(a.mapped_arg_value("topic_file")); - return goal; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_sayGoal goal; - goal.message.data = std::string(a.mapped_arg_value("say_string")); - return goal; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_lookAtGoal goal; - goal.position.push_back(double(a.mapped_arg_value("x"))); - goal.position.push_back(double(a.mapped_arg_value("y"))); - goal.position.push_back(double(a.mapped_arg_value("z"))); - goal.frame = int(a.mapped_arg_value("frame")); - goal.fractionMaxSpeed = int(a.mapped_arg_value("fractionMaxSpeed")); - goal.useWholeBody = bool(a.mapped_arg_value("useWholeBody")); - return goal; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_openWebsiteGoal goal; - goal.url.data = std::string(a.mapped_arg_value("url")); - goal.waitForWebCommand = bool(a.mapped_arg_value("waitForWebCommand")); - return goal; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_animatedSayGoal goal; - goal.animatedMessage.data = std::string(a.mapped_arg_value("animatedMessage")); - return goal; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_animationGoal goal; - goal.animation.data = std::string(a.mapped_arg_value("animation")); - return goal; -} - -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - naoqi_wrapper_msgs::NaoQi_subscribeGoal goal; - goal.eventName.data = std::string(a.mapped_arg_value("eventName")); - return goal; -} - -#endif // naoqi_wrapper_msgs_FOUND - -#ifdef darknet_action_msgs_FOUND -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - darknet::obj_detectionGoal goal; - goal.to_detected_obj = std::string(a.mapped_arg_value("to_detected_obj")); - return goal; -} -#endif // darknet_action_msgs_FOUND - -#ifdef move_base_msgs_FOUND -template<> -ActionManager::GoalT -ActionManager::build_goal(const gpp::Activity &a) -{ - move_base_msgs::MoveBaseGoal goal; - goal.target_pose.header.frame_id = std::string(a.mapped_arg_value("frame_id")); - goal.target_pose.header.stamp = ros::Time::now(); - goal.target_pose.pose.position.x = int(a.mapped_arg_value("x")); - goal.target_pose.pose.position.y = int(a.mapped_arg_value("y")); - goal.target_pose.pose.orientation.w = int(a.mapped_arg_value("w"));; - return goal; -} -#endif // move_base_msgs_FOUND - - -void RosBackend::define_actions() -{ -#ifdef naoqi_wrapper_msgs_FOUND - define_action_client("/naoqi_dialog_server"); - define_action_client("/naoqi_say_server/naoqi_say"); - define_action_client("/naoqi_lookAt_server/lookAt"); - define_action_client("/naoqi_openWebsite_server/openWebsite"); - define_action_client("/naoqi_animatedSay_server/animatedSay"); - define_action_client("/naoqi_animation_server/naoqi_animation"); - define_action_client("/naoqi_subscribe_server/subscribe"); -#endif - -#ifdef darknet_action_msgs_FOUND - define_action_client("/yolo_obj_detection_position_server"); -#endif - -#ifdef move_base_msgs_FOUND - define_action_client("move_base"); -#endif -} - - - -#ifdef naoqi_bridge_msgs_FOUND -template<> -void -ExogManager::topic_cb(const naoqi_bridge_msgs::Bumper::ConstPtr& msg) -{ - ROS_INFO_STREAM("I heard: " << bool(msg->statePressed)); - gpp::unique_ptr param (new gpp::Value(gpp::BoolType::name(), bool(msg->statePressed))); - exog_event_to_queue( - std::move(param) - ); -} -#endif // naoqi_bridge_msgs_FOUND - -void RosBackend::init_exog() -{ -#ifdef naoqi_bridge_msgs_FOUND - sub_exog_event( - "/pepper_robot/naoqi_driver/bumper" - ); -#endif -} - diff --git a/src/agent_node.cpp b/src/agent_node.cpp index e532403..723a9e0 100644 --- a/src/agent_node.cpp +++ b/src/agent_node.cpp @@ -1,89 +1,75 @@ -#include -#include -#include - -#include "ros_backend.h" - -#include -#include - #include #include #include +#include #include +#include -//#ifdef GOLOGPP_TEST_READYLOG +#include +#include +#include + +#include "gologpp_agent/ros_backend.h" + +// #ifdef GOLOGPP_TEST_READYLOG #include -//#endif +// #endif -//#ifdef GOLOGPP_TEST_PARSER +// #ifdef GOLOGPP_TEST_PARSER #include -//#endif +// #endif #include "rclcpp/rclcpp.hpp" - namespace gpp = gologpp; +void load_n_exec_program(std::string program) { + gpp::parser::parse_file(program); + gpp::shared_ptr> mainproc{ + gpp::global_scope().lookup_global("main")->make_ref({})}; -void load_n_exec_program(std::string program) -{ - gpp::parser::parse_file(program); + gpp::eclipse_opts options; + options.trace = false; + options.toplevel = false; + options.guitrace = false; + RosBackend* rosbackend = new RosBackend(); - gpp::shared_ptr> mainproc { - gpp::global_scope().lookup_global("main")->make_ref({}) - }; + gpp::ReadylogContext::init(options, gpp::unique_ptr(rosbackend)); + gpp::ReadylogContext& ctx = gpp::ReadylogContext::instance(); - gpp::eclipse_opts options; - options.trace = false; - options.toplevel = false; - options.guitrace = false; - RosBackend* rosbackend = new RosBackend(); + mainproc->attach_semantics(ctx.semantics_factory()); - gpp::ReadylogContext::init( - options, gpp::unique_ptr(rosbackend) - ); - gpp::ReadylogContext &ctx = gpp::ReadylogContext::instance(); - - mainproc->attach_semantics(ctx.semantics_factory()); - - rosbackend->ctx_ready = true; - ctx.run(*mainproc); - rosbackend->ctx_ready = false; + rosbackend->ctx_ready = true; + ctx.run(*mainproc); + rosbackend->ctx_ready = false; } +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + std::string param; + auto agent_node = Singleton::instance(); + std::string gpp_code = ""; + if ((argc <= 1) || (argv[argc - 1] == NULL)) { + // there is NO input... + gpp_code = "turtlesim_example"; + std::cerr << "No agent provided in the argument! Turtlesim example started!" << std::endl; + } else { + // there is an input... + gpp_code = argv[argc - 1]; + RCLCPP_INFO_STREAM(agent_node->get_logger(), "Starting Golog++ agent: " << gpp_code); + } -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - std::string param; - - auto agent_node = Singleton::instance(); - std::string gpp_code = ""; - - if ( (argc <= 1) || (argv[argc-1] == NULL) ) // there is NO input... - { - gpp_code = "turtlesim_example"; - std::cerr << "No agent provided in the argument! Turtlesim example started!" << std::endl; - } - else // there is an input... - { - gpp_code = argv[argc-1]; - RCLCPP_INFO_STREAM(agent_node->get_logger(), "Starting Golog++ agent: " << gpp_code); - } - - gpp_code = SOURCE_DIR "/agents/"+gpp_code+".gpp"; + gpp_code = SOURCE_DIR "/agents/" + gpp_code + ".gpp"; - if (!agent_node->has_parameter("gpp_code")) - agent_node->declare_parameter("gpp_code", gpp_code); - agent_node->get_parameter("gpp_code", gpp_code); + if (!agent_node->has_parameter("gpp_code")) agent_node->declare_parameter("gpp_code", gpp_code); + agent_node->get_parameter("gpp_code", gpp_code); - load_n_exec_program(gpp_code); + load_n_exec_program(gpp_code); - rclcpp::shutdown(); - gpp::ReadylogContext::shutdown(); + rclcpp::shutdown(); + gpp::ReadylogContext::shutdown(); - return 0; + return 0; } diff --git a/src/exog_manager.cpp b/src/exog_manager.cpp index 0f19641..b6e4e82 100644 --- a/src/exog_manager.cpp +++ b/src/exog_manager.cpp @@ -1,5 +1,3 @@ -#include "exog_manager.h" +#include "gologpp_agent/exog_manager.h" -AbstractExogManager::AbstractExogManager(RosBackend &backend) -: backend(backend) -{} +AbstractExogManager::AbstractExogManager(const RosBackend& backend) : backend(backend) {} diff --git a/src/exog_manager.h b/src/exog_manager.h deleted file mode 100644 index c3062bf..0000000 --- a/src/exog_manager.h +++ /dev/null @@ -1,98 +0,0 @@ -#pragma once - -#include "ros_backend.h" - -namespace gpp = gologpp; - -using std::placeholders::_1; - -class RosBackend; - - -class AbstractExogManager { -public: - AbstractExogManager(RosBackend &backend); - virtual ~AbstractExogManager() = default; - -protected: - RosBackend &backend; -}; - - -template -class ExogManager : public AbstractExogManager { -public: - ExogManager(RosBackend &backend, const std::string& topic, int msgs_queue_size = 1000); - - typename rclcpp::Subscription::SharedPtr exog_subscriber_; - - void topic_cb(const typename ExogT::ConstPtr&); - void exog_event_to_queue(std::unordered_map< std::string, gpp::unique_ptr > &¶ms_to_map); - std::unordered_map< std::string, gpp::unique_ptr > params_to_map(const typename ExogT::ConstPtr& msg); - -private: - gpp::shared_ptr< gpp::ExogAction> exog_; -}; - - -template -ExogManager::ExogManager(RosBackend &backend, const std::string& topic, int msgs_queue_size) -: AbstractExogManager (backend) -{ - auto agent_node = Singleton::instance(); - exog_subscriber_ = agent_node->create_subscription - (topic, 10, std::bind(&ExogManager::topic_cb, this, _1)); - - gpp::shared_ptr< gpp::ExogAction> exog; - std::vector< std::shared_ptr > global_vec = gpp::global_scope().globals(); - - for (std::vector>::iterator it = global_vec.begin(); - it != global_vec.end(); ++it){ - if((exog = std::dynamic_pointer_cast< gpp::ExogAction>(*it))){ - - if(exog->mapping().backend_name() == this->exog_subscriber_->get_topic_name()) { - exog_ = exog; - break; - } - } - } -} - - -template -void -ExogManager::topic_cb(const typename ExogT::ConstPtr& msg) -{ - if(backend.ctx_ready){ - //Topic msgs map to exog Event and pushed to Exog Queue - exog_event_to_queue( - params_to_map(msg) - ); - } -} - - -template -void ExogManager::exog_event_to_queue( std::unordered_map< std::string, gpp::unique_ptr > &¶ms_to_map) -{ - gpp::Binding param_binding; - - for (auto it = params_to_map.begin(); it != params_to_map.end(); it++){ - gpp::unique_ptr> param_ref { exog_->param_ref(it->first) }; - param_binding.bind(param_ref->target(), std::move(it->second)); - } - - gpp::shared_ptr ev { new gpp::ExogEvent(exog_, std::move(param_binding)) }; - - gpp::ReadylogContext &ctx = gpp::ReadylogContext::instance(); - ctx.exog_queue_push(ev); -} - - -template -void RosBackend::create_ExogManger(const std::string &topic) -{ - exog_managers_.push_back( - std::unique_ptr(new ExogManager(*this, topic)) - ); -} diff --git a/src/ros_backend.cpp b/src/ros_backend.cpp index ed20bfa..54a22b6 100644 --- a/src/ros_backend.cpp +++ b/src/ros_backend.cpp @@ -1,152 +1,116 @@ +#include "gologpp_agent/ros_backend.h" +#include "gologpp_agent/action_manager.h" +#include "gologpp_agent/exog_manager.h" -#include "ros_backend.h" -#include "action_manager.h" -#include "exog_manager.h" - - - -RosBackend::RosBackend() -{ - this->ctx_ready = false; +RosBackend::RosBackend() { + this->ctx_ready = false; #ifdef naoqi_wrapper_msgs_FOUND - define_naoqi_wrapper_actions(); + define_naoqi_wrapper_actions(); #endif #ifdef move_base_msgs_FOUND - define_move_base_actions(); + define_move_base_actions(); #endif #ifdef darknet_action_msgs_FOUND - define_darknet_actions(); + define_darknet_actions(); #endif #ifdef naoqi_bridge_msgs_FOUND - define_naoqi_bridge_actions(); + define_naoqi_bridge_actions(); #endif #ifdef opencv_apps_msgs_FOUND - define_opencv_apps_actions(); + define_opencv_apps_actions(); #endif #ifdef turtle_actionlib_FOUND - define_turtle_actions(); + define_turtle_actions(); #endif #ifdef turtlesim_FOUND - define_turtlesim_actions(); + define_turtlesim_actions(); #endif #ifdef gpp_action_examples_interface_FOUND - define_gpp_action_examples_interface_actions(); + define_gpp_action_examples_interface_actions(); #endif #ifdef webots_spot_msgs_FOUND - define_webots_spot_msgs_actions(); + define_webots_spot_msgs_actions(); #endif #ifdef nav2_msgs_FOUND - define_nav2_msgs_actions(); + define_nav2_msgs_actions(); #endif #ifdef spot_msgs_FOUND - define_spot_msgs_actions(); + define_spot_msgs_actions(); #endif - std::string built_interfaces_string = ""; - for (auto& name: built_interface_names) - { - built_interfaces_string += name + " "; - } - RCLCPP_INFO_STREAM(LOGGER, "Managers from " << built_interfaces_string << "are available"); - spin_exog_thread(); + std::string built_interfaces_string = ""; + for (auto& name : built_interface_names) { + built_interfaces_string += name + " "; + } + RCLCPP_INFO_STREAM(LOGGER, "Managers from " << built_interfaces_string << "are available"); + spin_exog_thread(); } +RosBackend::~RosBackend() {} -RosBackend::~RosBackend() -{} - - - -void RosBackend::preempt_activity(shared_ptr a) -{ - // TODO: Tell ActionManager to preempt - //gpp::shared_ptr a = std::make_shared(trans); - get_ActionManager(a).preempt(a); +void RosBackend::preempt_activity(shared_ptr a) { + get_ActionManager(a).preempt(a); } - -void RosBackend::execute_activity(shared_ptr a) -{ - // TODO: Find AbstractActionManager for a->mapped_name() - // and execute it, e.g.: - // action_manager.execute(a); - get_ActionManager(a).execute(a); +void RosBackend::execute_activity(shared_ptr a) { + get_ActionManager(a).execute(a); } +AbstractActionManager& RosBackend::get_ActionManager(shared_ptr a) { + auto it = action_managers_.find(std::string(a->mapped_name())); -AbstractActionManager& RosBackend::get_ActionManager(shared_ptr a) -{ - auto it = action_managers_.find(std::string(a->mapped_name())); - - if (it == action_managers_.end()) - { - RCLCPP_ERROR_STREAM(LOGGER, std::string(a->mapped_name()) << " is not defined"); - rclcpp::shutdown(); - exit(0); - } + if (it == action_managers_.end()) { + RCLCPP_ERROR_STREAM(LOGGER, std::string(a->mapped_name()) << " is not defined"); + rclcpp::shutdown(); + exit(0); + } - return *it->second.get(); + return *it->second.get(); } - -void RosBackend::spin_exog_thread() -{ - std::thread spin_thread( [&] () { - //ros::spin(); - auto agent_node = Singleton::instance(); - rclcpp::spin(agent_node); - }); - spin_thread.detach(); +void RosBackend::spin_exog_thread() { + std::thread spin_thread([&]() { + // ros::spin(); + auto agent_node = Singleton::instance(); + rclcpp::spin(agent_node); + }); + spin_thread.detach(); } - -gpp::Clock::time_point RosBackend::time() const noexcept -{ - Clock::duration rv = std::chrono::steady_clock::now().time_since_epoch(); - return Clock::time_point(rv); +gpp::Clock::time_point RosBackend::time() const noexcept { + Clock::duration rv = std::chrono::steady_clock::now().time_since_epoch(); + return Clock::time_point(rv); } +gpp::Value RosBackend::eval_exog_function(const Type& return_type, const string& backend_name, + const std::unordered_map& args) { + string act_name = static_cast(args.at("ros_action_name")); + AbstractActionManager& act_mgr = *action_managers_.at(act_name); -gpp::Value RosBackend::eval_exog_function( - const Type &return_type, - const string &backend_name, - const std::unordered_map &args -) { - string act_name = static_cast(args.at("ros_action_name")); - AbstractActionManager &act_mgr = *action_managers_.at(act_name); - - if (!act_mgr.current_activity()->target()->senses()) - throw gologpp::UserError( - backend_name + ": " + act_mgr.current_activity()->target()->str() - + " is not a sensing action" - ); - - auto opt_result = act_mgr.result(); - - if (!opt_result) - throw gologpp::UserError( - backend_name + ": " + act_mgr.current_activity()->str() - + " has not provided a sensing result" - ); - - if (opt_result.value().type() <= return_type) - return opt_result.value(); - else - throw gologpp::TypeError(opt_result.value(), return_type); -} + if (!act_mgr.current_activity()->target()->senses()) + throw gologpp::UserError(backend_name + ": " + act_mgr.current_activity()->target()->str() + + " is not a sensing action"); + + auto opt_result = act_mgr.result(); -void RosBackend::terminate_() -{ + if (!opt_result) + throw gologpp::UserError(backend_name + ": " + act_mgr.current_activity()->str() + + " has not provided a sensing result"); + if (opt_result.value().type() <= return_type) + return opt_result.value(); + else + throw gologpp::TypeError(opt_result.value(), return_type); } +void RosBackend::terminate_() {} diff --git a/src/ros_backend.h b/src/ros_backend.h deleted file mode 100644 index 3b87d27..0000000 --- a/src/ros_backend.h +++ /dev/null @@ -1,118 +0,0 @@ -#ifndef ROSBACKEND_H -#define ROSBACKEND_H - -// Remove spurious clang code model error -#ifdef Q_CREATOR_RUN -#undef __GCC_ASM_FLAG_OUTPUTS__ -#endif - -#define BOOST_BIND_GLOBAL_PLACEHOLDERS - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "rclcpp_components/register_node_macro.hpp" - -#include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("gpp_ros_backend"); - -class AbstractActionManager; -class AbstractExogManager; - -class Singleton -{ -public: - Singleton(Singleton const&) = delete; - Singleton& operator=(Singleton const&) = delete; - //std::shared_ptr node = rclcpp::Node::make_shared("gologpp_agent"); - - static std::shared_ptr instance() - { - static std::shared_ptr node = rclcpp::Node::make_shared("gologpp_agent"); - return node; - } - -private: - Singleton() {} -}; - -class RosBackend : public gologpp::PlatformBackend -{ -private: - using string = gologpp::string; - using Value = gologpp::Value; - using Type = gologpp::Type; - using Activity = gologpp::Activity; - using Clock = gologpp::Clock; - template using shared_ptr = gologpp::shared_ptr; - -public: - RosBackend(); - virtual ~RosBackend() override; - virtual void execute_activity(shared_ptr a) override; - virtual void preempt_activity(shared_ptr trans) override; - virtual Clock::time_point time() const noexcept override; - - virtual Value eval_exog_function( - const Type &return_type, - const string &backend_name, - const std::unordered_map &args - ) override; - - //std::mutex exog_mutex; - std::atomic ctx_ready; - -private: - virtual void terminate_() override; - - std::vector built_interface_names; - - // May have an implementation if the corresponding package has been found - void define_turtlesim_actions(); - void define_gpp_action_examples_interface_actions(); - void define_webots_spot_msgs_actions(); - void define_nav2_msgs_actions(); - void define_spot_msgs_actions(); - - template - void create_ActionManager(const std::string &name); - - AbstractActionManager &get_ActionManager(shared_ptr); - - // create exogManager - template - void create_ExogManger( - const std::string & - ); - - void spin_exog_thread(); - - template - void create_ServiceManager( - const std::string & - ); - - std::vector< - std::unique_ptr - > exog_managers_; - - std::unordered_map< - std::string, - std::unique_ptr - > action_managers_; -}; - -#endif // ROSBACKEND_H