diff --git a/cx_bringup/params/clips_features_manager.yaml b/cx_bringup/params/clips_features_manager.yaml index 674015d1..dd586481 100644 --- a/cx_bringup/params/clips_features_manager.yaml +++ b/cx_bringup/params/clips_features_manager.yaml @@ -1,6 +1,6 @@ clips_features_manager: ros__parameters: - clips_features_list: ["clips_pddl_parser", "skill_execution", "clips_protobuf", "ros_topic_publisher", "ros_topic_subscriber", "ros_service_requester"] + clips_features_list: ["clips_pddl_parser", "skill_execution", "clips_protobuf", "ros_topic_publisher", "ros_topic_subscriber", "ros_service_requester", "navgraph_compute", "navgraph_set_bounding_box"] clips_features: mock_feature: plugin: "cx::MockFeature" @@ -16,6 +16,14 @@ clips_features_manager: plugin: "cx::RosTopicSubscriberFeature" ros_service_requester: plugin: "cx::RosServiceRequesterFeature" + navgraph_compute: + plugin: "cx:NavGraphCompute" + navgraph_set_bounding_box: + plugin: "cx:NavGraphSetBoundingBox" + navgraph_set_bounding_box: + plugin: "cx:NavGraphGenerateWaitzones" + navgraph_set_bounding_box: + plugin: "cx:NavGraphUpdateStationByTag" clips_protobuf: plugin: "cx::ClipsProtobufFeature" feature_parameters: ["protobuf_path"] diff --git a/cx_features/CMakeLists.txt b/cx_features/CMakeLists.txt index 0a47af76..28942d49 100644 --- a/cx_features/CMakeLists.txt +++ b/cx_features/CMakeLists.txt @@ -95,6 +95,22 @@ add_library(ros_topic_subscriber_feature SHARED src/cx_features/ros_communicatio ament_target_dependencies(ros_topic_subscriber_feature ${dependencies}) target_link_libraries(ros_topic_subscriber_feature PkgConfig::CLIPSMM) +add_library(navgraph_compute SHARED src/cx_features/ros_communication_feature/NavGraphCompute.cpp) +ament_target_dependencies(navgraph_compute ${dependencies}) +target_link_libraries(navgraph_compute PkgConfig::CLIPSMM) + +add_library(navgraph_set_bounding_box SHARED src/cx_features/ros_communication_feature/NavGraphSetBoundingBox.cpp) +ament_target_dependencies(navgraph_set_bounding_box ${dependencies}) +target_link_libraries(navgraph_set_bounding_box PkgConfig::CLIPSMM) + +add_library(navgraph_generate_waitzones SHARED src/cx_features/ros_communication_feature/NavGraphGenerateWaitzones.cpp) +ament_target_dependencies(navgraph_generate_waitzones ${dependencies}) +target_link_libraries(navgraph_generate_waitzones PkgConfig::CLIPSMM) + +add_library(navgraph_update_station_by_tag SHARED src/cx_features/ros_communication_feature/NavGraphUpdateStationByTag.cpp) +ament_target_dependencies(navgraph_update_station_by_tag ${dependencies}) +target_link_libraries(navgraph_update_station_by_tag PkgConfig::CLIPSMM) + # END OF PLUGINS REGISTRATION add_library(${PROJECT_NAME} SHARED ${CX_FEATURES_SOURCES}) @@ -128,6 +144,10 @@ install(TARGETS ros_service_requester_feature ros_topic_subscriber_feature ros_topic_publisher_feature + navgraph_compute + navgraph_set_bounding_box + navgraph_generate_waitzones + navgraph_update_station_by_tag clips_pddl_parser_feature clips_protobuf_feature ${PROJECT_NAME} diff --git a/cx_features/features_plugin.xml b/cx_features/features_plugin.xml index 2d9eea2f..b0d772fa 100644 --- a/cx_features/features_plugin.xml +++ b/cx_features/features_plugin.xml @@ -39,4 +39,24 @@ Clips Feature providing Protobuf-based communication + + + Clips Feature providing access to NavGraphInterfaceCompute ROS2 services + + + + + Clips Feature providing access to NavGraphInterfaceSetBoundingBox ROS2 services + + + + + Clips Feature providing access to NavGraphInterfaceGenerateWaitzones ROS2 services + + + + + Clips Feature providing access to NavGraphInterfaceUpdateStationByTag ROS2 services + + \ No newline at end of file diff --git a/cx_features/include/cx_features/NavGraphCompute.hpp b/cx_features/include/cx_features/NavGraphCompute.hpp new file mode 100644 index 00000000..20c671de --- /dev/null +++ b/cx_features/include/cx_features/NavGraphCompute.hpp @@ -0,0 +1,79 @@ +/*************************************************************************** + * NavGraphCompute.hpp + * + * Automatically Generated: 2023-11-30 14:32:29 + ****************************************************************************/ + +/* This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Library General Public License for more details. + * + * Read the full text in the LICENSE.GPL file in the doc directory. + */ + +#ifndef CX_FEATURES__NAVGRAPHCOMPUTE_HPP_ +#define CX_FEATURES__NAVGRAPHCOMPUTE_HPP_ + +#include +#include +#include + +#include "cx_core/ClipsFeature.hpp" +#include "cx_utils/LockSharedPtr.hpp" +#include "cx_utils/NodeThread.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "cx_msgs/srv/nav_graph_interface_compute.hpp" + +namespace cx { + +class NavGraphCompute : public ClipsFeature, public rclcpp::Node { +public: + NavGraphCompute(); + ~NavGraphCompute(); + + void initialise(const std::string &feature_name) override; + + bool clips_context_init(const std::string &env_name, + LockSharedPtr &clips) override; + bool clips_context_destroyed(const std::string &env_name) override; + + std::string getFeatureName() const; + +private: + std::map> envs_; + std::thread spin_thread_; + std::map< + std::string, + std::map::SharedPtr>> + request_clients_; + std::map< + std::string, + std::map>> + requests_; + void create_request(const std::string &env_name, + const std::string &service_name); + + void set_field_request(const std::string &env_name, + const std::string &service_name, + const std::string &field_name, CLIPS::Value value); + + void set_array_request(const std::string &env_name, + const std::string &service_name, + const std::string &field_name, CLIPS::Values values); + + void request_from_node(const std::string &env_name, + const std::string &service_name); + + void service_callback( + rclcpp::Client::SharedFuture response, + std::string service_name, std::string env_name); +}; +} // namespace cx +#endif // !CX_FEATURES__NAVGRAPHCOMPUTE_HPP_ \ No newline at end of file diff --git a/cx_features/include/cx_features/NavGraphGenerateWaitzones.hpp b/cx_features/include/cx_features/NavGraphGenerateWaitzones.hpp new file mode 100644 index 00000000..9498232b --- /dev/null +++ b/cx_features/include/cx_features/NavGraphGenerateWaitzones.hpp @@ -0,0 +1,79 @@ +/*************************************************************************** + * NavGraphGenerateWaitzones.hpp + * + * Automatically Generated: 2023-11-30 14:38:37 + ****************************************************************************/ + +/* This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Library General Public License for more details. + * + * Read the full text in the LICENSE.GPL file in the doc directory. + */ + +#ifndef CX_FEATURES__NAVGRAPHGENERATEWAITZONES_HPP_ +#define CX_FEATURES__NAVGRAPHGENERATEWAITZONES_HPP_ + +#include +#include +#include + +#include "cx_core/ClipsFeature.hpp" +#include "cx_utils/LockSharedPtr.hpp" +#include "cx_utils/NodeThread.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "cx_msgs/srv/nav_graph_interface_generate_waitzones.hpp" + +namespace cx { + +class NavGraphGenerateWaitzones : public ClipsFeature, public rclcpp::Node { +public: + NavGraphGenerateWaitzones(); + ~NavGraphGenerateWaitzones(); + + void initialise(const std::string &feature_name) override; + + bool clips_context_init(const std::string &env_name, + LockSharedPtr &clips) override; + bool clips_context_destroyed(const std::string &env_name) override; + + std::string getFeatureName() const; + +private: + std::map> envs_; + std::thread spin_thread_; + std::map< + std::string, + std::map::SharedPtr>> + request_clients_; + std::map< + std::string, + std::map>> + requests_; + void create_request(const std::string &env_name, + const std::string &service_name); + + void set_field_request(const std::string &env_name, + const std::string &service_name, + const std::string &field_name, CLIPS::Value value); + + void set_array_request(const std::string &env_name, + const std::string &service_name, + const std::string &field_name, CLIPS::Values values); + + void request_from_node(const std::string &env_name, + const std::string &service_name); + + void service_callback( + rclcpp::Client::SharedFuture response, + std::string service_name, std::string env_name); +}; +} // namespace cx +#endif // !CX_FEATURES__NAVGRAPHGENERATEWAITZONES_HPP_ \ No newline at end of file diff --git a/cx_features/include/cx_features/NavGraphSetBoundingBox.hpp b/cx_features/include/cx_features/NavGraphSetBoundingBox.hpp new file mode 100644 index 00000000..1098afea --- /dev/null +++ b/cx_features/include/cx_features/NavGraphSetBoundingBox.hpp @@ -0,0 +1,79 @@ +/*************************************************************************** + * NavGraphSetBoundingBox.hpp + * + * Automatically Generated: 2023-11-30 14:34:21 + ****************************************************************************/ + +/* This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Library General Public License for more details. + * + * Read the full text in the LICENSE.GPL file in the doc directory. + */ + +#ifndef CX_FEATURES__NAVGRAPHSETBOUNDINGBOX_HPP_ +#define CX_FEATURES__NAVGRAPHSETBOUNDINGBOX_HPP_ + +#include +#include +#include + +#include "cx_core/ClipsFeature.hpp" +#include "cx_utils/LockSharedPtr.hpp" +#include "cx_utils/NodeThread.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "cx_msgs/srv/nav_graph_interface_set_bounding_box.hpp" + +namespace cx { + +class NavGraphSetBoundingBox : public ClipsFeature, public rclcpp::Node { +public: + NavGraphSetBoundingBox(); + ~NavGraphSetBoundingBox(); + + void initialise(const std::string &feature_name) override; + + bool clips_context_init(const std::string &env_name, + LockSharedPtr &clips) override; + bool clips_context_destroyed(const std::string &env_name) override; + + std::string getFeatureName() const; + +private: + std::map> envs_; + std::thread spin_thread_; + std::map< + std::string, + std::map::SharedPtr>> + request_clients_; + std::map< + std::string, + std::map>> + requests_; + void create_request(const std::string &env_name, + const std::string &service_name); + + void set_field_request(const std::string &env_name, + const std::string &service_name, + const std::string &field_name, CLIPS::Value value); + + void set_array_request(const std::string &env_name, + const std::string &service_name, + const std::string &field_name, CLIPS::Values values); + + void request_from_node(const std::string &env_name, + const std::string &service_name); + + void service_callback( + rclcpp::Client::SharedFuture response, + std::string service_name, std::string env_name); +}; +} // namespace cx +#endif // !CX_FEATURES__NAVGRAPHSETBOUNDINGBOX_HPP_ \ No newline at end of file diff --git a/cx_features/include/cx_features/NavGraphUpdateStationByTag.hpp b/cx_features/include/cx_features/NavGraphUpdateStationByTag.hpp new file mode 100644 index 00000000..e9dff35d --- /dev/null +++ b/cx_features/include/cx_features/NavGraphUpdateStationByTag.hpp @@ -0,0 +1,79 @@ +/*************************************************************************** + * NavGraphUpdateStationByTag.hpp + * + * Automatically Generated: 2023-11-30 14:55:13 + ****************************************************************************/ + +/* This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Library General Public License for more details. + * + * Read the full text in the LICENSE.GPL file in the doc directory. + */ + +#ifndef CX_FEATURES__NAVGRAPHUPDATESTATIONBYTAG_HPP_ +#define CX_FEATURES__NAVGRAPHUPDATESTATIONBYTAG_HPP_ + +#include +#include +#include + +#include "cx_core/ClipsFeature.hpp" +#include "cx_utils/LockSharedPtr.hpp" +#include "cx_utils/NodeThread.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "cx_msgs/srv/nav_graph_interface_update_station_by_tag.hpp" + +namespace cx { + +class NavGraphUpdateStationByTag : public ClipsFeature, public rclcpp::Node { +public: + NavGraphUpdateStationByTag(); + ~NavGraphUpdateStationByTag(); + + void initialise(const std::string &feature_name) override; + + bool clips_context_init(const std::string &env_name, + LockSharedPtr &clips) override; + bool clips_context_destroyed(const std::string &env_name) override; + + std::string getFeatureName() const; + +private: + std::map> envs_; + std::thread spin_thread_; + std::map< + std::string, + std::map::SharedPtr>> + request_clients_; + std::map< + std::string, + std::map>> + requests_; + void create_request(const std::string &env_name, + const std::string &service_name); + + void set_field_request(const std::string &env_name, + const std::string &service_name, + const std::string &field_name, CLIPS::Value value); + + void set_array_request(const std::string &env_name, + const std::string &service_name, + const std::string &field_name, CLIPS::Values values); + + void request_from_node(const std::string &env_name, + const std::string &service_name); + + void service_callback( + rclcpp::Client::SharedFuture response, + std::string service_name, std::string env_name); +}; +} // namespace cx +#endif // !CX_FEATURES__NAVGRAPHUPDATESTATIONBYTAG_HPP_ \ No newline at end of file diff --git a/cx_features/src/cx_features/ros_communication_feature/NavGraphCompute.cpp b/cx_features/src/cx_features/ros_communication_feature/NavGraphCompute.cpp new file mode 100644 index 00000000..9c8f89f9 --- /dev/null +++ b/cx_features/src/cx_features/ros_communication_feature/NavGraphCompute.cpp @@ -0,0 +1,179 @@ +/*************************************************************************** + * NavGraphCompute.cpp + * + * Automatically Generated: 2023-11-30 14:32:29 + ****************************************************************************/ + +/* This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Library General Public License for more details. + * + * Read the full text in the LICENSE.GPL file in the doc directory. + */ + +#include +#include +#include + +#include "cx_core/ClipsFeature.hpp" +#include "cx_features/NavGraphCompute.hpp" +#include "cx_utils/LockSharedPtr.hpp" + +// To export as plugin +#include "pluginlib/class_list_macros.hpp" + +using std::placeholders::_1; + +namespace cx { + +NavGraphCompute::NavGraphCompute() + : Node("ros_service_requester_feature_node") {} +NavGraphCompute::~NavGraphCompute() {} + +std::string NavGraphCompute::getFeatureName() const { + return clips_feature_name; +} + +void NavGraphCompute::initialise(const std::string &feature_name) { + clips_feature_name = feature_name; + + spin_thread_ = + std::thread([this]() { rclcpp::spin(this->get_node_base_interface()); }); +} + +bool NavGraphCompute::clips_context_init( + const std::string &env_name, LockSharedPtr &clips) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Initialising context for feature %s", + clips_feature_name.c_str()); + + envs_[env_name] = clips; + + // add base implementations for ros communication + // all of these need to be implemented given the corresponding types + clips->add_function( + "ros-nav_graph_compute-create-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphCompute::create_request), + env_name))); + clips->add_function( + "ros-nav_graph_compute-set-field-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphCompute::set_field_request), + env_name))); + clips->add_function( + "ros-nav_graph_compute-set-array-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphCompute::set_array_request), + env_name))); + clips->add_function( + "ros-nav_graph_compute-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphCompute::request_from_node), + env_name))); + + // add base fact templates + clips->build("(deftemplate ros-nav_graph_compute-response\ + (slot service (type STRING)) \ + (slot success (type STRING)) \ + )"); + + return true; +} + +bool NavGraphCompute::clips_context_destroyed( + const std::string &env_name) { + + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Destroying clips context!"); + envs_.erase(env_name); + + return true; +} + +void NavGraphCompute::create_request( + const std::string &env_name, const std::string &service_name) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Creating request for service %s %s", service_name.c_str(), + env_name.c_str()); + + request_clients_[env_name][service_name] = + this->create_client(service_name); + requests_[env_name][service_name] = + std::make_shared(); +} + +void NavGraphCompute::set_field_request( + const std::string &env_name, const std::string &service_name, + const std::string &field_name, CLIPS::Value value) { + (void)service_name; + (void)env_name; + (void)value; + (void)field_name; +} + +void NavGraphCompute::set_array_request( + const std::string &env_name, const std::string &service_name, + const std::string &field_name, CLIPS::Values values) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Setting array %s for service %s %s not supported ", + field_name.c_str(), service_name.c_str(), env_name.c_str()); + (void)service_name; + (void)field_name; + (void)env_name; + (void)values; +} + +void NavGraphCompute::request_from_node( + const std::string &env_name, const std::string &service_name) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Requesting service %s %s", service_name.c_str(), + env_name.c_str()); + + auto response_callback = + [=](rclcpp::Client::SharedFuture response) { + service_callback(response, service_name, env_name); + }; + request_clients_[env_name][service_name]->async_send_request( + requests_[env_name][service_name], response_callback); +} + +void NavGraphCompute::service_callback( + rclcpp::Client::SharedFuture response, + std::string service_name, std::string env_name) { + cx::LockSharedPtr &clips = envs_[env_name]; + std::lock_guard guard(*(clips.get_mutex_instance())); + + // remove old responses facts + std::vector facts = {}; + CLIPS::Fact::pointer old_fact = envs_[env_name]->get_facts(); + while (old_fact) { + if (old_fact->get_template()->name() == "ros-nav_graph_compute-response" && + old_fact->slot_value("service")[0].as_string() == service_name) { + facts.push_back(old_fact); + } + old_fact = old_fact->next(); + } + for (auto &old_fact : facts) { + old_fact->retract(); + } + + // assert the newest responses + auto result = response.get(); + CLIPS::Template::pointer fact_template = envs_[env_name]->get_template("ros-nav_graph_compute-message"); + CLIPS::Fact::pointer fact = CLIPS::Fact::create(*(envs_[env_name].get_obj()), fact_template); + + fact->set_slot("success", result->success ? "TRUE" : "FALSE"); + fact->set_slot("service", service_name); + + envs_[env_name]->assert_fact(fact); +} + +} // namespace cx +PLUGINLIB_EXPORT_CLASS(cx::NavGraphCompute, cx::ClipsFeature) \ No newline at end of file diff --git a/cx_features/src/cx_features/ros_communication_feature/NavGraphGenerateWaitzones.cpp b/cx_features/src/cx_features/ros_communication_feature/NavGraphGenerateWaitzones.cpp new file mode 100644 index 00000000..6ec5792f --- /dev/null +++ b/cx_features/src/cx_features/ros_communication_feature/NavGraphGenerateWaitzones.cpp @@ -0,0 +1,179 @@ +/*************************************************************************** + * NavGraphGenerateWaitzones.cpp + * + * Automatically Generated: 2023-11-30 14:38:37 + ****************************************************************************/ + +/* This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Library General Public License for more details. + * + * Read the full text in the LICENSE.GPL file in the doc directory. + */ + +#include +#include +#include + +#include "cx_core/ClipsFeature.hpp" +#include "cx_features/NavGraphGenerateWaitzones.hpp" +#include "cx_utils/LockSharedPtr.hpp" + +// To export as plugin +#include "pluginlib/class_list_macros.hpp" + +using std::placeholders::_1; + +namespace cx { + +NavGraphGenerateWaitzones::NavGraphGenerateWaitzones() + : Node("ros_service_requester_feature_node") {} +NavGraphGenerateWaitzones::~NavGraphGenerateWaitzones() {} + +std::string NavGraphGenerateWaitzones::getFeatureName() const { + return clips_feature_name; +} + +void NavGraphGenerateWaitzones::initialise(const std::string &feature_name) { + clips_feature_name = feature_name; + + spin_thread_ = + std::thread([this]() { rclcpp::spin(this->get_node_base_interface()); }); +} + +bool NavGraphGenerateWaitzones::clips_context_init( + const std::string &env_name, LockSharedPtr &clips) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Initialising context for feature %s", + clips_feature_name.c_str()); + + envs_[env_name] = clips; + + // add base implementations for ros communication + // all of these need to be implemented given the corresponding types + clips->add_function( + "ros-nav_graph_generate_waitzones-create-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphGenerateWaitzones::create_request), + env_name))); + clips->add_function( + "ros-nav_graph_generate_waitzones-set-field-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphGenerateWaitzones::set_field_request), + env_name))); + clips->add_function( + "ros-nav_graph_generate_waitzones-set-array-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphGenerateWaitzones::set_array_request), + env_name))); + clips->add_function( + "ros-nav_graph_generate_waitzones-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphGenerateWaitzones::request_from_node), + env_name))); + + // add base fact templates + clips->build("(deftemplate ros-nav_graph_generate_waitzones-response\ + (slot service (type STRING)) \ + (slot success (type STRING)) \ + )"); + + return true; +} + +bool NavGraphGenerateWaitzones::clips_context_destroyed( + const std::string &env_name) { + + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Destroying clips context!"); + envs_.erase(env_name); + + return true; +} + +void NavGraphGenerateWaitzones::create_request( + const std::string &env_name, const std::string &service_name) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Creating request for service %s %s", service_name.c_str(), + env_name.c_str()); + + request_clients_[env_name][service_name] = + this->create_client(service_name); + requests_[env_name][service_name] = + std::make_shared(); +} + +void NavGraphGenerateWaitzones::set_field_request( + const std::string &env_name, const std::string &service_name, + const std::string &field_name, CLIPS::Value value) { + (void)service_name; + (void)env_name; + (void)value; + (void)field_name; +} + +void NavGraphGenerateWaitzones::set_array_request( + const std::string &env_name, const std::string &service_name, + const std::string &field_name, CLIPS::Values values) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Setting array %s for service %s %s not supported ", + field_name.c_str(), service_name.c_str(), env_name.c_str()); + (void)service_name; + (void)field_name; + (void)env_name; + (void)values; +} + +void NavGraphGenerateWaitzones::request_from_node( + const std::string &env_name, const std::string &service_name) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Requesting service %s %s", service_name.c_str(), + env_name.c_str()); + + auto response_callback = + [=](rclcpp::Client::SharedFuture response) { + service_callback(response, service_name, env_name); + }; + request_clients_[env_name][service_name]->async_send_request( + requests_[env_name][service_name], response_callback); +} + +void NavGraphGenerateWaitzones::service_callback( + rclcpp::Client::SharedFuture response, + std::string service_name, std::string env_name) { + cx::LockSharedPtr &clips = envs_[env_name]; + std::lock_guard guard(*(clips.get_mutex_instance())); + + // remove old responses facts + std::vector facts = {}; + CLIPS::Fact::pointer old_fact = envs_[env_name]->get_facts(); + while (old_fact) { + if (old_fact->get_template()->name() == "ros-nav_graph_generate_waitzones-response" && + old_fact->slot_value("service")[0].as_string() == service_name) { + facts.push_back(old_fact); + } + old_fact = old_fact->next(); + } + for (auto &old_fact : facts) { + old_fact->retract(); + } + + // assert the newest responses + auto result = response.get(); + CLIPS::Template::pointer fact_template = envs_[env_name]->get_template("ros-nav_graph_generate_waitzones-message"); + CLIPS::Fact::pointer fact = CLIPS::Fact::create(*(envs_[env_name].get_obj()), fact_template); + + fact->set_slot("success", result->success ? "TRUE" : "FALSE"); + fact->set_slot("service", service_name); + + envs_[env_name]->assert_fact(fact); +} + +} // namespace cx +PLUGINLIB_EXPORT_CLASS(cx::NavGraphGenerateWaitzones, cx::ClipsFeature) \ No newline at end of file diff --git a/cx_features/src/cx_features/ros_communication_feature/NavGraphSetBoundingBox.cpp b/cx_features/src/cx_features/ros_communication_feature/NavGraphSetBoundingBox.cpp new file mode 100644 index 00000000..f676641f --- /dev/null +++ b/cx_features/src/cx_features/ros_communication_feature/NavGraphSetBoundingBox.cpp @@ -0,0 +1,187 @@ +/*************************************************************************** + * NavGraphSetBoundingBox.cpp + * + * Automatically Generated: 2023-11-30 14:34:21 + ****************************************************************************/ + +/* This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Library General Public License for more details. + * + * Read the full text in the LICENSE.GPL file in the doc directory. + */ + +#include +#include +#include + +#include "cx_core/ClipsFeature.hpp" +#include "cx_features/NavGraphSetBoundingBox.hpp" +#include "cx_utils/LockSharedPtr.hpp" + +// To export as plugin +#include "pluginlib/class_list_macros.hpp" + +using std::placeholders::_1; + +namespace cx { + +NavGraphSetBoundingBox::NavGraphSetBoundingBox() + : Node("ros_service_requester_feature_node") {} +NavGraphSetBoundingBox::~NavGraphSetBoundingBox() {} + +std::string NavGraphSetBoundingBox::getFeatureName() const { + return clips_feature_name; +} + +void NavGraphSetBoundingBox::initialise(const std::string &feature_name) { + clips_feature_name = feature_name; + + spin_thread_ = + std::thread([this]() { rclcpp::spin(this->get_node_base_interface()); }); +} + +bool NavGraphSetBoundingBox::clips_context_init( + const std::string &env_name, LockSharedPtr &clips) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Initialising context for feature %s", + clips_feature_name.c_str()); + + envs_[env_name] = clips; + + // add base implementations for ros communication + // all of these need to be implemented given the corresponding types + clips->add_function( + "ros-nav_graph_set_bounding_box-create-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphSetBoundingBox::create_request), + env_name))); + clips->add_function( + "ros-nav_graph_set_bounding_box-set-field-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphSetBoundingBox::set_field_request), + env_name))); + clips->add_function( + "ros-nav_graph_set_bounding_box-set-array-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphSetBoundingBox::set_array_request), + env_name))); + clips->add_function( + "ros-nav_graph_set_bounding_box-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphSetBoundingBox::request_from_node), + env_name))); + + // add base fact templates + clips->build("(deftemplate ros-nav_graph_set_bounding_box-response\ + (slot service (type STRING)) \ + (slot success (type STRING)) \ + )"); + + return true; +} + +bool NavGraphSetBoundingBox::clips_context_destroyed( + const std::string &env_name) { + + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Destroying clips context!"); + envs_.erase(env_name); + + return true; +} + +void NavGraphSetBoundingBox::create_request( + const std::string &env_name, const std::string &service_name) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Creating request for service %s %s", service_name.c_str(), + env_name.c_str()); + + request_clients_[env_name][service_name] = + this->create_client(service_name); + requests_[env_name][service_name] = + std::make_shared(); +} + +void NavGraphSetBoundingBox::set_field_request( + const std::string &env_name, const std::string &service_name, + const std::string &field_name, CLIPS::Value value) { + if (field_name == "p1_x") { + requests_[env_name][service_name]->p1_x = value.as_float(); + } + if (field_name == "p1_y") { + requests_[env_name][service_name]->p1_y = value.as_float(); + } + if (field_name == "p2_x") { + requests_[env_name][service_name]->p2_x = value.as_float(); + } + if (field_name == "p2_y") { + requests_[env_name][service_name]->p2_y = value.as_float(); + } +} + +void NavGraphSetBoundingBox::set_array_request( + const std::string &env_name, const std::string &service_name, + const std::string &field_name, CLIPS::Values values) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Setting array %s for service %s %s not supported ", + field_name.c_str(), service_name.c_str(), env_name.c_str()); + (void)service_name; + (void)field_name; + (void)env_name; + (void)values; +} + +void NavGraphSetBoundingBox::request_from_node( + const std::string &env_name, const std::string &service_name) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Requesting service %s %s", service_name.c_str(), + env_name.c_str()); + + auto response_callback = + [=](rclcpp::Client::SharedFuture response) { + service_callback(response, service_name, env_name); + }; + request_clients_[env_name][service_name]->async_send_request( + requests_[env_name][service_name], response_callback); +} + +void NavGraphSetBoundingBox::service_callback( + rclcpp::Client::SharedFuture response, + std::string service_name, std::string env_name) { + cx::LockSharedPtr &clips = envs_[env_name]; + std::lock_guard guard(*(clips.get_mutex_instance())); + + // remove old responses facts + std::vector facts = {}; + CLIPS::Fact::pointer old_fact = envs_[env_name]->get_facts(); + while (old_fact) { + if (old_fact->get_template()->name() == "ros-nav_graph_set_bounding_box-response" && + old_fact->slot_value("service")[0].as_string() == service_name) { + facts.push_back(old_fact); + } + old_fact = old_fact->next(); + } + for (auto &old_fact : facts) { + old_fact->retract(); + } + + // assert the newest responses + auto result = response.get(); + CLIPS::Template::pointer fact_template = envs_[env_name]->get_template("ros-nav_graph_set_bounding_box-message"); + CLIPS::Fact::pointer fact = CLIPS::Fact::create(*(envs_[env_name].get_obj()), fact_template); + + fact->set_slot("success", result->success ? "TRUE" : "FALSE"); + fact->set_slot("service", service_name); + + envs_[env_name]->assert_fact(fact); +} + +} // namespace cx +PLUGINLIB_EXPORT_CLASS(cx::NavGraphSetBoundingBox, cx::ClipsFeature) \ No newline at end of file diff --git a/cx_features/src/cx_features/ros_communication_feature/NavGraphUpdateStationByTag.cpp b/cx_features/src/cx_features/ros_communication_feature/NavGraphUpdateStationByTag.cpp new file mode 100644 index 00000000..4bfa026c --- /dev/null +++ b/cx_features/src/cx_features/ros_communication_feature/NavGraphUpdateStationByTag.cpp @@ -0,0 +1,201 @@ +/*************************************************************************** + * NavGraphUpdateStationByTag.cpp + * + * Automatically Generated: 2023-11-30 14:55:13 + ****************************************************************************/ + +/* This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Library General Public License for more details. + * + * Read the full text in the LICENSE.GPL file in the doc directory. + */ + +#include +#include +#include + +#include "cx_core/ClipsFeature.hpp" +#include "cx_features/NavGraphUpdateStationByTag.hpp" +#include "cx_utils/LockSharedPtr.hpp" + +// To export as plugin +#include "pluginlib/class_list_macros.hpp" + +using std::placeholders::_1; + +namespace cx { + +NavGraphUpdateStationByTag::NavGraphUpdateStationByTag() + : Node("ros_service_requester_feature_node") {} +NavGraphUpdateStationByTag::~NavGraphUpdateStationByTag() {} + +std::string NavGraphUpdateStationByTag::getFeatureName() const { + return clips_feature_name; +} + +void NavGraphUpdateStationByTag::initialise(const std::string &feature_name) { + clips_feature_name = feature_name; + + spin_thread_ = + std::thread([this]() { rclcpp::spin(this->get_node_base_interface()); }); +} + +bool NavGraphUpdateStationByTag::clips_context_init( + const std::string &env_name, LockSharedPtr &clips) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Initialising context for feature %s", + clips_feature_name.c_str()); + + envs_[env_name] = clips; + + // add base implementations for ros communication + // all of these need to be implemented given the corresponding types + clips->add_function( + "ros-nav_graph_update_station_by_tag-create-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphUpdateStationByTag::create_request), + env_name))); + clips->add_function( + "ros-nav_graph_update_station_by_tag-set-field-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphUpdateStationByTag::set_field_request), + env_name))); + clips->add_function( + "ros-nav_graph_update_station_by_tag-set-array-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphUpdateStationByTag::set_array_request), + env_name))); + clips->add_function( + "ros-nav_graph_update_station_by_tag-request", + sigc::slot(sigc::bind<0>( + sigc::mem_fun(*this, &NavGraphUpdateStationByTag::request_from_node), + env_name))); + + // add base fact templates + clips->build("(deftemplate ros-nav_graph_update_station_by_tag-response\ + (slot service (type STRING)) \ + (slot success (type STRING)) \ + )"); + + return true; +} + +bool NavGraphUpdateStationByTag::clips_context_destroyed( + const std::string &env_name) { + + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Destroying clips context!"); + envs_.erase(env_name); + + return true; +} + +void NavGraphUpdateStationByTag::create_request( + const std::string &env_name, const std::string &service_name) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Creating request for service %s %s", service_name.c_str(), + env_name.c_str()); + + request_clients_[env_name][service_name] = + this->create_client(service_name); + requests_[env_name][service_name] = + std::make_shared(); +} + +void NavGraphUpdateStationByTag::set_field_request( + const std::string &env_name, const std::string &service_name, + const std::string &field_name, CLIPS::Value value) { + if (field_name == "name") { + requests_[env_name][service_name]->name = value.as_string(); + } + if (field_name == "side") { + requests_[env_name][service_name]->side = value.as_string(); + } + if (field_name == "frame") { + requests_[env_name][service_name]->frame = value.as_string(); + } +} + +void NavGraphUpdateStationByTag::set_array_request( + const std::string &env_name, const std::string &service_name, + const std::string &field_name, CLIPS::Values values) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Setting array %s for service %s %s not supported ", + field_name.c_str(), service_name.c_str(), env_name.c_str()); + if (field_name == "tag_translation") { + std::vector values_vector_tag_translation; + for (auto value : values) { + values_vector_tag_translation.push_back(value.as_float()); + } + requests_[env_name][service_name]->tag_translation = values_vector_tag_translation; + } + if (field_name == "tag_rotation") { + std::vector values_vector_tag_rotation; + for (auto value : values) { + values_vector_tag_rotation.push_back(value.as_float()); + } + requests_[env_name][service_name]->tag_rotation = values_vector_tag_rotation; + } + if (field_name == "zone_coords") { + std::vector values_vector_zone_coords; + for (auto value : values) { + values_vector_zone_coords.push_back(value.as_integer()); + } + requests_[env_name][service_name]->zone_coords = values_vector_zone_coords; + } +} + +void NavGraphUpdateStationByTag::request_from_node( + const std::string &env_name, const std::string &service_name) { + RCLCPP_INFO(rclcpp::get_logger(clips_feature_name), + "Requesting service %s %s", service_name.c_str(), + env_name.c_str()); + + auto response_callback = + [=](rclcpp::Client::SharedFuture response) { + service_callback(response, service_name, env_name); + }; + request_clients_[env_name][service_name]->async_send_request( + requests_[env_name][service_name], response_callback); +} + +void NavGraphUpdateStationByTag::service_callback( + rclcpp::Client::SharedFuture response, + std::string service_name, std::string env_name) { + cx::LockSharedPtr &clips = envs_[env_name]; + std::lock_guard guard(*(clips.get_mutex_instance())); + + // remove old responses facts + std::vector facts = {}; + CLIPS::Fact::pointer old_fact = envs_[env_name]->get_facts(); + while (old_fact) { + if (old_fact->get_template()->name() == "ros-nav_graph_update_station_by_tag-response" && + old_fact->slot_value("service")[0].as_string() == service_name) { + facts.push_back(old_fact); + } + old_fact = old_fact->next(); + } + for (auto &old_fact : facts) { + old_fact->retract(); + } + + // assert the newest responses + auto result = response.get(); + CLIPS::Template::pointer fact_template = envs_[env_name]->get_template("ros-nav_graph_update_station_by_tag-message"); + CLIPS::Fact::pointer fact = CLIPS::Fact::create(*(envs_[env_name].get_obj()), fact_template); + + fact->set_slot("success", result->success ? "TRUE" : "FALSE"); + fact->set_slot("service", service_name); + + envs_[env_name]->assert_fact(fact); +} + +} // namespace cx +PLUGINLIB_EXPORT_CLASS(cx::NavGraphUpdateStationByTag, cx::ClipsFeature) \ No newline at end of file diff --git a/cx_msgs/CMakeLists.txt b/cx_msgs/CMakeLists.txt index 4d7db78f..884b3867 100644 --- a/cx_msgs/CMakeLists.txt +++ b/cx_msgs/CMakeLists.txt @@ -18,6 +18,8 @@ set(MSGS "msg/PddlGenInterfaceMessages.msg" "msg/SkillExecution.msg" "msg/SkillActionExecInfo.msg" + "msg/NavGraphInterfaceMessage.msg" + "msg/NavGraphWithMPSInterfaceMessage.msg" ) set(SERVICES @@ -28,6 +30,10 @@ set(SERVICES "srv/ClipsRemoveFeatures.srv" "srv/PddlGenInterfaceSrv.srv" "srv/OpenInterface.srv" + "srv/NavGraphInterfaceCompute.srv" + "srv/NavGraphInterfaceGenerateWaitzones.srv" + "srv/NavGraphInterfaceSetBoundingBox.srv" + "srv/NavGraphInterfaceUpdateStationByTag.srv" ) # set(ACTIONS diff --git a/cx_msgs/msg/NavGraphInterfaceMessage.msg b/cx_msgs/msg/NavGraphInterfaceMessage.msg new file mode 100644 index 00000000..ef7894e1 --- /dev/null +++ b/cx_msgs/msg/NavGraphInterfaceMessage.msg @@ -0,0 +1,5 @@ +# Feedback from Fawke's NavGraphGeneratorInterface + +bool final +bool ok +string<=1024 error_msg diff --git a/cx_msgs/msg/NavGraphWithMPSInterfaceMessage.msg b/cx_msgs/msg/NavGraphWithMPSInterfaceMessage.msg new file mode 100644 index 00000000..69fdb0ba --- /dev/null +++ b/cx_msgs/msg/NavGraphWithMPSInterfaceMessage.msg @@ -0,0 +1,3 @@ +# Feedback from Fawke's NavGraphWithMPSGeneratorInterface + +bool final diff --git a/cx_msgs/srv/NavGraphInterfaceCompute.srv b/cx_msgs/srv/NavGraphInterfaceCompute.srv new file mode 100644 index 00000000..1308cc21 --- /dev/null +++ b/cx_msgs/srv/NavGraphInterfaceCompute.srv @@ -0,0 +1,2 @@ +--- +bool success \ No newline at end of file diff --git a/cx_msgs/srv/NavGraphInterfaceGenerateWaitzones.srv b/cx_msgs/srv/NavGraphInterfaceGenerateWaitzones.srv new file mode 100644 index 00000000..1308cc21 --- /dev/null +++ b/cx_msgs/srv/NavGraphInterfaceGenerateWaitzones.srv @@ -0,0 +1,2 @@ +--- +bool success \ No newline at end of file diff --git a/cx_msgs/srv/NavGraphInterfaceSetBoundingBox.srv b/cx_msgs/srv/NavGraphInterfaceSetBoundingBox.srv new file mode 100644 index 00000000..eb2030d4 --- /dev/null +++ b/cx_msgs/srv/NavGraphInterfaceSetBoundingBox.srv @@ -0,0 +1,6 @@ +float32 p1_x +float32 p1_y +float32 p2_x +float32 p2_y +--- +bool success diff --git a/cx_msgs/srv/NavGraphInterfaceUpdateStationByTag.srv b/cx_msgs/srv/NavGraphInterfaceUpdateStationByTag.srv new file mode 100644 index 00000000..e955081f --- /dev/null +++ b/cx_msgs/srv/NavGraphInterfaceUpdateStationByTag.srv @@ -0,0 +1,11 @@ +string name +string side +string frame +float64[] tag_translation +# (x,y,z) -> coordinate from frame origin +float64[] tag_rotation +# (x,y,z,w) -> Quaternion rotation from frame +int16[] zone_coords +# (x,y) -> Integral zone coordinates, i.e. [2,3] for C-Z23 or [-2,3] for M-Z23. +--- +bool success