Merge branch 'main' of https://github.com/dawan0111/yacyac into featu…

…re/servo_ctrl

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

yacyac_core/bt_xml/bt_nav_yac_supply.xml

@@ -28,23 +28,24 @@
 
             <ForceSuccess>
               <Sequence>
-                <Nav2Client goal="{Goal_a}"/>
-                <QRClient />
+                <!-- <Nav2Client goal="{Goal_a}"/> -->
+                <QRClient user_id="{user_id}" />
+                <SupplyAcceptClient />
                 <YacSupplyCilent goal="{yac_a}"/>
               </Sequence>
             </ForceSuccess>
 
             <ForceSuccess>
               <Sequence>
-                <Nav2Client goal="{Goal_b}"/>
+                <!-- <Nav2Client goal="{Goal_b}"/> -->
                 <QRClient />
                 <YacSupplyCilent goal="{yac_b}"/>
               </Sequence>
             </ForceSuccess>
 
             <ForceSuccess>
               <Sequence>
-                <Nav2Client goal="{Goal_c}"/>
+                <!-- <Nav2Client goal="{Goal_c}"/> -->
                 <QRClient />
                 <YacSupplyCilent goal="{yac_c}"/>
               </Sequence>

yacyac_core/include/yacyac_core/qr_client.hpp

@@ -2,8 +2,8 @@
 #define QR_CLIENT_HPP_
 
 #include <behaviortree_cpp/behavior_tree.h>
-#include <std_msgs/msg/int8.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/int8.hpp>
 
 #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<std::string>("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<std::string> detected_QR_;
 
-    void QR_callback_(const yacyac_interface::msg::Qrcode::ConstSharedPtr& msg);
+    void QR_callback_(const yacyac_interface::msg::Qrcode& msg);
 };
 
 #endif

yacyac_core/include/yacyac_core/supply_accept_client.hpp

@@ -0,0 +1,31 @@
+#ifndef SUPPLY_FEEDBACK_CLIENT_HPP_
+#define SUPPLY_FEEDBACK_CLIENT_HPP_
+
+#include <behaviortree_cpp/behavior_tree.h>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+
+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<std_msgs::msg::Bool>::SharedPtr feedback_sub_;
+    bool supply_accept_;
+    bool has_supply_accept_;
+};
+
+#endif

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 <ament_index_cpp/get_package_share_directory.hpp>
 #include <behaviortree_cpp/bt_factory.h>
 #include <behaviortree_cpp/loggers/bt_cout_logger.h>
@@ -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>("Nav2Client");
     factory.registerNodeType<YacSupplyCilent>("YacSupplyCilent");
-    // std::cout << "둥" << std::endl;
-    // factory.registerNodeType<QRClient>("QRClient");
-    // factory.registerNodeType<Message>("Message");
+    factory.registerNodeType<QRClient>("QRClient");
+    factory.registerNodeType<Message>("Message");
+    factory.registerNodeType<SupplyAcceptClient>("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

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<yacyac_interface::msg::Qrcode>(QR_topic_name, rclcpp::QoS(1), std::bind(&QRClient::QR_callback_, this, std::placeholders::_1));
-
-    mode_publisher_ = node_->create_publisher<std_msgs::msg::Int8>(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<yacyac_interface::msg::Qrcode>(QR_topic_name, rclcpp::QoS(1), std::bind(&QRClient::QR_callback_, this, std::placeholders::_1));
+    mode_publisher_ = node_->create_publisher<std_msgs::msg::Int8>(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;
 }

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<std_msgs::msg::Bool>(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 ]");
+}