From 11a43b66e6c043f71714afe12777b2db425b3697 Mon Sep 17 00:00:00 2001 From: ggh Date: Sat, 26 Aug 2023 22:41:22 +0900 Subject: [PATCH 1/3] add supplyAcceptClient --- yacyac_core/CMakeLists.txt | 1 + .../yacyac_core/supply_accept_client.hpp | 31 +++++++++++++ yacyac_core/src/supply_accept_client.cpp | 45 +++++++++++++++++++ 3 files changed, 77 insertions(+) create mode 100644 yacyac_core/include/yacyac_core/supply_accept_client.hpp create mode 100644 yacyac_core/src/supply_accept_client.cpp diff --git a/yacyac_core/CMakeLists.txt b/yacyac_core/CMakeLists.txt index 7048969..e73b066 100644 --- a/yacyac_core/CMakeLists.txt +++ b/yacyac_core/CMakeLists.txt @@ -46,6 +46,7 @@ add_library( src/qr_client.cpp src/message.cpp src/nav2_client.cpp + src/supply_accept_client.cpp ) # send_goal diff --git a/yacyac_core/include/yacyac_core/supply_accept_client.hpp b/yacyac_core/include/yacyac_core/supply_accept_client.hpp new file mode 100644 index 0000000..6eb2815 --- /dev/null +++ b/yacyac_core/include/yacyac_core/supply_accept_client.hpp @@ -0,0 +1,31 @@ +#ifndef SUPPLY_FEEDBACK_CLIENT_HPP_ +#define SUPPLY_FEEDBACK_CLIENT_HPP_ + +#include +#include +#include + +class SupplyAcceptClient : public BT::StatefulActionNode { +public: + SupplyAcceptClient(const std::string& name, const BT::NodeConfig& config); + ~SupplyAcceptClient(); + + static BT::PortsList providedPorts() { return {}; } + + // this function is invoked once at the beginning. + BT::NodeStatus onStart() override; + + // If onStart() returned RUNNING, we will keep calling + // this method until it return something different from RUNNING + BT::NodeStatus onRunning() override; + + void onHalted() override; + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr feedback_sub_; + bool supply_accept_; + bool has_supply_accept_; +}; + +#endif diff --git a/yacyac_core/src/supply_accept_client.cpp b/yacyac_core/src/supply_accept_client.cpp new file mode 100644 index 0000000..9e3a001 --- /dev/null +++ b/yacyac_core/src/supply_accept_client.cpp @@ -0,0 +1,45 @@ +#include "yacyac_core/supply_accept_client.hpp" + +SupplyAcceptClient::SupplyAcceptClient(const std::string& name, const BT::NodeConfig& config) : BT::StatefulActionNode(name, config) {} + +SupplyAcceptClient::~SupplyAcceptClient() +{ + RCLCPP_INFO(node_->get_logger(), "SHUTTING DOWN SupplyAcceptClient NODE"); +} + +BT::NodeStatus SupplyAcceptClient::onStart() +{ + node_ = rclcpp::Node::make_shared("supply_accept_client"); + + const std::string QR_topic_name = "/user_feedback"; + + has_supply_accept_ = false; + + feedback_sub_ = node_->create_subscription(QR_topic_name, rclcpp::QoS(1), [this](const std_msgs::msg::Bool::SharedPtr msg) { + supply_accept_ = msg->data; + has_supply_accept_ = true; + }); + + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus SupplyAcceptClient::onRunning() +{ + rclcpp::spin_some(node_); + if (has_supply_accept_) { + if (supply_accept_) { + return BT::NodeStatus::SUCCESS; + } + else { + return BT::NodeStatus::FAILURE; + } + } + else { + return BT::NodeStatus::RUNNING; + } +} + +void SupplyAcceptClient::onHalted() +{ + // printf("[ SupplyAcceptClient: ABORTED ]"); +} \ No newline at end of file From b40934751e773f1e08dbdba8a8f98a2e8d3db14d Mon Sep 17 00:00:00 2001 From: ggh Date: Sat, 26 Aug 2023 22:42:03 +0900 Subject: [PATCH 2/3] qr_client subscriber error bug fix --- yacyac_core/include/yacyac_core/qr_client.hpp | 6 ++-- yacyac_core/src/qr_client.cpp | 28 ++++++++++--------- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/yacyac_core/include/yacyac_core/qr_client.hpp b/yacyac_core/include/yacyac_core/qr_client.hpp index 00f52a8..3bfc9e2 100644 --- a/yacyac_core/include/yacyac_core/qr_client.hpp +++ b/yacyac_core/include/yacyac_core/qr_client.hpp @@ -2,8 +2,8 @@ #define QR_CLIENT_HPP_ #include -#include #include +#include #include "yacyac_interface/msg/qrcode.hpp" @@ -12,7 +12,7 @@ class QRClient : public BT::StatefulActionNode { QRClient(const std::string& name, const BT::NodeConfig& config); ~QRClient(); - static BT::PortsList providedPorts() { return {}; } + static BT::PortsList providedPorts() { return { BT::OutputPort("user_id") }; } // this function is invoked once at the beginning. BT::NodeStatus onStart() override; @@ -30,7 +30,7 @@ class QRClient : public BT::StatefulActionNode { double deadline_; std::vector detected_QR_; - void QR_callback_(const yacyac_interface::msg::Qrcode::ConstSharedPtr& msg); + void QR_callback_(const yacyac_interface::msg::Qrcode& msg); }; #endif diff --git a/yacyac_core/src/qr_client.cpp b/yacyac_core/src/qr_client.cpp index 6588cbf..b80e112 100644 --- a/yacyac_core/src/qr_client.cpp +++ b/yacyac_core/src/qr_client.cpp @@ -1,14 +1,6 @@ #include "yacyac_core/qr_client.hpp" -QRClient::QRClient(const std::string& name, const BT::NodeConfig& config) : BT::StatefulActionNode(name, config) -{ - node_ = rclcpp::Node::make_shared("qr_client"); - const std::string QR_topic_name = "/qr_codes"; - const std::string mode_topic_name = "/mode"; - node_->create_subscription(QR_topic_name, rclcpp::QoS(1), std::bind(&QRClient::QR_callback_, this, std::placeholders::_1)); - - mode_publisher_ = node_->create_publisher(mode_topic_name, rclcpp::QoS(1)); -} +QRClient::QRClient(const std::string& name, const BT::NodeConfig& config) : BT::StatefulActionNode(name, config) {} QRClient::~QRClient() { @@ -17,7 +9,15 @@ QRClient::~QRClient() BT::NodeStatus QRClient::onStart() { - double life_time = rclcpp::Duration(3, 0).seconds(); + node_ = rclcpp::Node::make_shared("qr_client"); + + const std::string QR_topic_name = "/qr_node"; + const std::string mode_topic_name = "/mode"; + + qr_sub_ = node_->create_subscription(QR_topic_name, rclcpp::QoS(1), std::bind(&QRClient::QR_callback_, this, std::placeholders::_1)); + mode_publisher_ = node_->create_publisher(mode_topic_name, rclcpp::QoS(1)); + + double life_time = rclcpp::Duration(30, 0).seconds(); deadline_ = node_->get_clock()->now().seconds() + life_time; auto message = std_msgs::msg::Int8(); @@ -29,12 +29,13 @@ BT::NodeStatus QRClient::onStart() BT::NodeStatus QRClient::onRunning() { - // rclcpp::spin_some(node_); + rclcpp::spin_some(node_); if (node_->get_clock()->now().seconds() <= deadline_) { int8_t detected_QR_length = detected_QR_.size(); // RCLCPP_INFO(node_->get_logger(), "detected qr length: %d", detected_QR_length); if (detected_QR_length == 1) { + setOutput("user_id", detected_QR_[0]); return BT::NodeStatus::SUCCESS; } else if (detected_QR_length > 1) { @@ -52,7 +53,8 @@ void QRClient::onHalted() // printf("[ QRClient: ABORTED ]"); } -void QRClient::QR_callback_(const yacyac_interface::msg::Qrcode::ConstSharedPtr& msg) +void QRClient::QR_callback_(const yacyac_interface::msg::Qrcode& msg) { - detected_QR_ = msg->qr_infos; + // RCLCPP_INFO(node_->get_logger(), "detected qr length: %d", msg.qr_infos.size()); + detected_QR_ = msg.qr_infos; } \ No newline at end of file From d40cc7227740989fbb3de44c4acf497635cd27ae Mon Sep 17 00:00:00 2001 From: ggh Date: Sat, 26 Aug 2023 22:42:15 +0900 Subject: [PATCH 3/3] bt update --- yacyac_core/bt_xml/bt_nav_yac_supply.xml | 9 +++++---- yacyac_core/src/bt_ros2.cpp | 7 ++++--- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/yacyac_core/bt_xml/bt_nav_yac_supply.xml b/yacyac_core/bt_xml/bt_nav_yac_supply.xml index c98e62c..3ff008d 100644 --- a/yacyac_core/bt_xml/bt_nav_yac_supply.xml +++ b/yacyac_core/bt_xml/bt_nav_yac_supply.xml @@ -28,15 +28,16 @@ - - + + + - + @@ -44,7 +45,7 @@ - + diff --git a/yacyac_core/src/bt_ros2.cpp b/yacyac_core/src/bt_ros2.cpp index 36a1e7f..5b29bac 100644 --- a/yacyac_core/src/bt_ros2.cpp +++ b/yacyac_core/src/bt_ros2.cpp @@ -1,10 +1,10 @@ #include "nav2_client.cpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "yac_supply_client.cpp" #include "yacyac_core/message.hpp" #include "yacyac_core/qr_client.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" +#include "yacyac_core/supply_accept_client.hpp" #include #include #include @@ -38,6 +38,7 @@ int main(int argc, char** argv) factory.registerNodeType("YacSupplyCilent"); factory.registerNodeType("QRClient"); factory.registerNodeType("Message"); + factory.registerNodeType("SupplyAcceptClient"); // Trees are created at deployment-time (i.e. at run-time, but only once at // the beginning). The currently supported format is XML. IMPORTANT: when the