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/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/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/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/bt_ros2.cpp b/yacyac_core/src/bt_ros2.cpp
index 6729332..93dd12d 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
@@ -36,9 +36,9 @@ int main(int argc, char** argv)
RCLCPP_INFO(nh->get_logger(), "Loading XML : %s", bt_xml.c_str());
// factory.registerNodeType("Nav2Client");
factory.registerNodeType("YacSupplyCilent");
- // std::cout << "둥" << std::endl;
- // factory.registerNodeType("QRClient");
- // factory.registerNodeType("Message");
+ 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
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
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