Feature/bringup

yacyac_bringup/launch/robot_description.launch.py

@@ -23,15 +23,15 @@ def generate_launch_description():
     )
 
     # # tracer mini package
-    # yacyac_camera_prefix = get_package_share_directory("yacyac_camera")
-    # start_yacyac_camera_cmd = IncludeLaunchDescription(
-    #     PythonLaunchDescriptionSource(os.path.join(yacyac_camera_prefix, "launch", "camera.launch.py"))
-    # )
+    yacyac_camera_prefix = get_package_share_directory("yacyac_camera")
+    start_yacyac_camera_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(yacyac_camera_prefix, "launch", "camera.launch.py"))
+    )
 
     # # rosbridge websocket cmd
-    # start_web_bridge_cmd = ExecuteProcess(
-    #     cmd=["ros2", "launch", "rosbridge_server", "rosbridge_websocket_launch.xml"], output="screen"
-    # )
+    start_web_bridge_cmd = ExecuteProcess(
+        cmd=["ros2", "launch", "rosbridge_server", "rosbridge_websocket_launch.xml"], output="screen"
+    )
 
     # start_insta360_cmd = ExecuteProcess(
     #     cmd=["ros2", "run", "insta360_node", "insta360_node"], output="screen"
@@ -81,7 +81,7 @@ def generate_launch_description():
             base_to_imu_publisher,
             start_g2lidar_cmd,
             start_yacyac_cmd,
-            # start_web_bridge_cmd,
-            # start_yacyac_camera_cmd
+            start_web_bridge_cmd,
+            start_yacyac_camera_cmd
         ]
     )

yacyac_core/CMakeLists.txt

@@ -33,9 +33,9 @@ set(DEPENDENCIES
   rclcpp_action
   std_msgs
   geometry_msgs
-  nav2_behavior_tree
   nav2_msgs
   behaviortree_cpp
+  nav2_behavior_tree
   yacyac_interface
   ${OTHER_DEPS}
 )  

yacyac_core/bt_xml/bt_nav_yac_supply.xml

@@ -1,6 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<root BTCPP_format="4"
-      main_tree_to_execute="MainTree">
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
   <BehaviorTree ID="Init">
     <Sequence>
       <SetBlackboard value="-0.579;-1.341;0.0;1.0"

yacyac_core/include/yacyac_core/qr_client.hpp

@@ -2,6 +2,7 @@
 #define QR_CLIENT_HPP_
 
 #include <behaviortree_cpp/behavior_tree.h>
+#include <std_msgs/msg/int8.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include "yacyac_interface/msg/qrcode.hpp"
@@ -25,6 +26,7 @@ class QRClient : public BT::StatefulActionNode {
 private:
     rclcpp::Node::SharedPtr node_;
     rclcpp::Subscription<yacyac_interface::msg::Qrcode>::SharedPtr qr_sub_;
+    rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr mode_publisher_;
     double deadline_;
     std::vector<std::string> detected_QR_;
 

yacyac_core/src/nav2_client.cpp

@@ -16,6 +16,7 @@
 #include "tf2/LinearMath/Quaternion.h"
 #include "tf2/transform_datatypes.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include <std_msgs/msg/int8.hpp>
 
 // Custom type
 struct Pose2D {
@@ -56,8 +57,17 @@ class Nav2Client : public BT::SyncActionNode {
     virtual BT::NodeStatus tick() override
     {
         std::cout << "nav2 client tick" << std::endl;
+        const std::string mode_topic_name = "/mode";
+
         node_ = rclcpp::Node::make_shared("nav2_client");
         auto action_client = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(node_, "navigate_to_pose");
+
+        auto mode_publisher = node_->create_publisher<std_msgs::msg::Int8>(mode_topic_name, rclcpp::QoS(1).best_effort());
+
+        auto message = std_msgs::msg::Int8();
+        message.data = 1;
+
+        mode_publisher->publish(message);
         // if no server is present, fail after 5 seconds
         std::cout << "nav2 server wait" << std::endl;
         if (!action_client->wait_for_action_server(std::chrono::seconds(20))) {

yacyac_core/src/qr_client.cpp

@@ -3,8 +3,11 @@
 QRClient::QRClient(const std::string& name, const BT::NodeConfig& config) : BT::StatefulActionNode(name, config)
 {
     node_ = rclcpp::Node::make_shared("qr_client");
-    std::string QR_topic_name = "/qr_codes";
+    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).best_effort());
 }
 
 QRClient::~QRClient()
@@ -14,9 +17,13 @@ QRClient::~QRClient()
 
 BT::NodeStatus QRClient::onStart()
 {
-    double life_time = rclcpp::Duration(10, 0).seconds();
+    double life_time = rclcpp::Duration(30, 0).seconds();
     deadline_ = node_->get_clock()->now().seconds() + life_time;
 
+    auto message = std_msgs::msg::Int8();
+    message.data = 1;
+    mode_publisher_->publish(message);
+
     return BT::NodeStatus::RUNNING;
 }