Merge pull request #44 from dawan0111/feature/last

last
dawan0111 · Aug 27, 2023 · 9fa5f25 · 9fa5f25
2 parents 6931505 + 0996a16
commit 9fa5f25
Show file tree

Hide file tree

Showing 238 changed files with 92 additions and 46 deletions.
diff --git a/.clang-format b/.clang-format
diff --git a/.github/workflows/code-format.yaml b/.github/workflows/code-format.yaml
diff --git a/.gitignore b/.gitignore
diff --git a/.gitmodules b/.gitmodules
diff --git a/DynamixelSDK/.gitattributes b/DynamixelSDK/.gitattributes
diff --git a/DynamixelSDK/.github/ISSUE_TEMPLATE.md b/DynamixelSDK/.github/ISSUE_TEMPLATE.md
diff --git a/DynamixelSDK/.github/workflows/ros-ci.yml b/DynamixelSDK/.github/workflows/ros-ci.yml
diff --git a/DynamixelSDK/.gitignore b/DynamixelSDK/.gitignore
diff --git a/DynamixelSDK/CONTRIBUTING.md b/DynamixelSDK/CONTRIBUTING.md
diff --git a/DynamixelSDK/Doxyfile b/DynamixelSDK/Doxyfile
diff --git a/DynamixelSDK/LICENSE b/DynamixelSDK/LICENSE
diff --git a/DynamixelSDK/README.md b/DynamixelSDK/README.md
diff --git a/DynamixelSDK/dynamixel_sdk/CHANGELOG.rst b/DynamixelSDK/dynamixel_sdk/CHANGELOG.rst
diff --git a/DynamixelSDK/dynamixel_sdk/CMakeLists.txt b/DynamixelSDK/dynamixel_sdk/CMakeLists.txt
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/dynamixel_sdk.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/dynamixel_sdk.h
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler.h
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_arduino.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_arduino.h
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_linux.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_linux.h
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_mac.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_mac.h
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_windows.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_windows.h
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h
diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h
diff --git a/DynamixelSDK/dynamixel_sdk/package.xml b/DynamixelSDK/dynamixel_sdk/package.xml
diff --git a/DynamixelSDK/dynamixel_sdk/src/DynamixelSDK.h b/DynamixelSDK/dynamixel_sdk/src/DynamixelSDK.h
diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp
diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp
diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp
diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp
diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp
diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp
diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp
diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp
diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_mac.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_mac.cpp
diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_windows.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_windows.cpp
diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp
diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp
diff --git a/DynamixelSDK/dynamixel_sdk_custom_interfaces/CHANGELOG.rst b/DynamixelSDK/dynamixel_sdk_custom_interfaces/CHANGELOG.rst
diff --git a/DynamixelSDK/dynamixel_sdk_custom_interfaces/CMakeLists.txt b/DynamixelSDK/dynamixel_sdk_custom_interfaces/CMakeLists.txt
diff --git a/DynamixelSDK/dynamixel_sdk_custom_interfaces/msg/SetPosition.msg b/DynamixelSDK/dynamixel_sdk_custom_interfaces/msg/SetPosition.msg
diff --git a/DynamixelSDK/dynamixel_sdk_custom_interfaces/package.xml b/DynamixelSDK/dynamixel_sdk_custom_interfaces/package.xml
diff --git a/DynamixelSDK/dynamixel_sdk_custom_interfaces/srv/GetPosition.srv b/DynamixelSDK/dynamixel_sdk_custom_interfaces/srv/GetPosition.srv
diff --git a/DynamixelSDK/dynamixel_sdk_examples/CHANGELOG.rst b/DynamixelSDK/dynamixel_sdk_examples/CHANGELOG.rst
diff --git a/DynamixelSDK/dynamixel_sdk_examples/CMakeLists.txt b/DynamixelSDK/dynamixel_sdk_examples/CMakeLists.txt
diff --git a/DynamixelSDK/dynamixel_sdk_examples/include/read_write_node.hpp b/DynamixelSDK/dynamixel_sdk_examples/include/read_write_node.hpp
diff --git a/DynamixelSDK/dynamixel_sdk_examples/package.xml b/DynamixelSDK/dynamixel_sdk_examples/package.xml
diff --git a/DynamixelSDK/dynamixel_sdk_examples/src/read_write_node.cpp b/DynamixelSDK/dynamixel_sdk_examples/src/read_write_node.cpp
diff --git a/md/CMakeLists.txt b/md/CMakeLists.txt
diff --git a/md/include/md/com.hpp b/md/include/md/com.hpp
diff --git a/md/include/md/global.hpp b/md/include/md/global.hpp
diff --git a/md/launch/md.launch.py b/md/launch/md.launch.py
diff --git a/md/package.xml b/md/package.xml
diff --git a/md/param/md_param.yaml b/md/param/md_param.yaml
@@ -7,10 +7,10 @@ md_ros2_param:
     reduction: 1 
     reverse_direction: 0
     maxrpm: 400
-    motor_posi: 0
+    motor_posi: 1
     encoder_PPR: 4096
     position_proportion_gain: 20
     speed_proportion_gain: 50
     integral_gain: 1800
     slow_start: 300
-    slow_down: 300 
+    slow_down: 300 
diff --git a/md/src/com.cpp b/md/src/com.cpp
diff --git a/md/src/main.cpp b/md/src/main.cpp
@@ -329,7 +329,7 @@ void cmdVelCallBack(const geometry_msgs::msg::Twist::SharedPtr keyVel) // from t
     if (fgInitsetting == INIT_SETTING_STATE_OK) {
         velCmdUpdateCount++;
 
-        goal_cmd_speed = keyVel->linear.x * -1;
+        goal_cmd_speed = keyVel->linear.x;
         goal_cmd_ang_speed = keyVel->angular.z;
     }
     return;
@@ -380,7 +380,7 @@ int main(int argc, char** argv)
     node->declare_parameter("reduction", 1);
     node->declare_parameter("reverse_direction", 0);
     node->declare_parameter("maxrpm", 400);
-    node->declare_parameter("motor_posi", 0);
+    node->declare_parameter("motor_posi", 1);
     node->declare_parameter("encoder_PPR", 4096);
     node->declare_parameter("position_proportion_gain", 20);
     node->declare_parameter("speed_proportion_gain", 50);
@@ -579,6 +579,7 @@ void PubRobotPose(void)
 
         robot_pose.platform_state = temp.val;
     }
+
 
     // robot_pose.battery_voltage = curr_pid_robot_monitor2.sVoltIn;
     // robot_pose.left_mot_current = curr_pid_pnt_main_data.current_id1;
@@ -606,4 +607,4 @@ void PubRobotPose(void)
 
     robot_pose_pub->publish(robot_pose);
     robot_odom_pub->publish(robot_odom);
-}
+}
diff --git a/md/src/robot.cpp b/md/src/robot.cpp
@@ -121,7 +121,7 @@ void CalRobotPoseFromRPM(PID_PNT_MAIN_DATA_t* pData)
     double robot_curr_theta;
 
     // auto curr_time = ros::Time::now();
-    auto curr_time = rclcpp::Clock{}.now();
+    auto curr_time = rclcpp::Clock().now();
 
     //----------------------------------------------------------------
     rpm_left = pData->rpm_id1;
@@ -183,6 +183,10 @@ void CalRobotPoseFromRPM(PID_PNT_MAIN_DATA_t* pData)
     robot_odom.pose.pose.orientation.y = quat.y();
     robot_odom.pose.pose.orientation.z = quat.z();
     robot_odom.pose.pose.orientation.w = quat.w();
+
+    robot_odom.twist.twist.linear.x = pVelocity[0];
+    robot_odom.twist.twist.linear.y = 0.0;
+    robot_odom.twist.twist.angular.z = pVelocity[1];
 }
 
 void CalRobotPoseFromPos(PID_PNT_MAIN_DATA_t* pData)
@@ -210,7 +214,7 @@ void CalRobotPoseFromPos(PID_PNT_MAIN_DATA_t* pData)
     double angular_vel;
 
     // ros::Time curr_time = ros::Time::now();
-    auto curr_time = rclcpp::Clock{}.now();
+    auto curr_time = rclcpp::Clock().now();
 
     pos_left = pData->mtr_pos_id1;
     pos_right = pData->mtr_pos_id2;
@@ -276,6 +280,10 @@ void CalRobotPoseFromPos(PID_PNT_MAIN_DATA_t* pData)
     robot_odom.pose.pose.orientation.y = quat.y();
     robot_odom.pose.pose.orientation.z = quat.z();
     robot_odom.pose.pose.orientation.w = quat.w();
+
+    robot_odom.twist.twist.linear.x = linear_vel;
+    robot_odom.twist.twist.linear.y = 0.0;
+    robot_odom.twist.twist.angular.z = angular_vel;
 }
 
 void CalRobotPose_old(PID_PNT_MAIN_DATA_t* pData)
@@ -295,7 +303,7 @@ void CalRobotPose_old(PID_PNT_MAIN_DATA_t* pData)
     double robot_curr_theta;
 
     // ros::Time curr_time = ros::Time::now();
-    auto curr_time = rclcpp::Clock{}.now();
+    auto curr_time = rclcpp::Clock().now();
 
     //----------------------------------------------------------------
     rpm_left = pData->rpm_id1;
@@ -364,4 +372,8 @@ void CalRobotPose_old(PID_PNT_MAIN_DATA_t* pData)
     robot_odom.pose.pose.orientation.y = quat.y();
     robot_odom.pose.pose.orientation.z = quat.z();
     robot_odom.pose.pose.orientation.w = quat.w();
+
+    robot_odom.twist.twist.linear.x = pVelocity[0];
+    robot_odom.twist.twist.linear.y = 0.0;
+    robot_odom.twist.twist.angular.z = pVelocity[1];
 }
diff --git a/serial-ros2/CHANGELOG.rst b/serial-ros2/CHANGELOG.rst
diff --git a/serial-ros2/CMakeLists.txt b/serial-ros2/CMakeLists.txt
diff --git a/serial-ros2/Makefile b/serial-ros2/Makefile
diff --git a/serial-ros2/README.md b/serial-ros2/README.md
diff --git a/serial-ros2/changes.txt b/serial-ros2/changes.txt
diff --git a/serial-ros2/doc/Doxyfile b/serial-ros2/doc/Doxyfile
diff --git a/serial-ros2/examples/serial_example.cc b/serial-ros2/examples/serial_example.cc
diff --git a/serial-ros2/include/serial/impl/unix.h b/serial-ros2/include/serial/impl/unix.h
diff --git a/serial-ros2/include/serial/impl/win.h b/serial-ros2/include/serial/impl/win.h
diff --git a/serial-ros2/include/serial/serial.h b/serial-ros2/include/serial/serial.h
diff --git a/serial-ros2/include/serial/v8stdint.h b/serial-ros2/include/serial/v8stdint.h
diff --git a/serial-ros2/package.xml b/serial-ros2/package.xml
diff --git a/serial-ros2/serial.sublime-project b/serial-ros2/serial.sublime-project
diff --git a/serial-ros2/src/impl/list_ports/list_ports_linux.cc b/serial-ros2/src/impl/list_ports/list_ports_linux.cc
diff --git a/serial-ros2/src/impl/list_ports/list_ports_osx.cc b/serial-ros2/src/impl/list_ports/list_ports_osx.cc
diff --git a/serial-ros2/src/impl/list_ports/list_ports_win.cc b/serial-ros2/src/impl/list_ports/list_ports_win.cc
diff --git a/serial-ros2/src/impl/win.cc b/serial-ros2/src/impl/win.cc
diff --git a/serial-ros2/visual_studio/serial/serial.vcxproj b/serial-ros2/visual_studio/serial/serial.vcxproj
diff --git a/serial-ros2/visual_studio/serial/serial.vcxproj.filters b/serial-ros2/visual_studio/serial/serial.vcxproj.filters
diff --git a/serial-ros2/visual_studio/test_serial/test_serial.vcxproj b/serial-ros2/visual_studio/test_serial/test_serial.vcxproj
diff --git a/serial-ros2/visual_studio/test_serial/test_serial.vcxproj.filters b/serial-ros2/visual_studio/test_serial/test_serial.vcxproj.filters
diff --git a/serial-ros2/visual_studio/visual_studio.sln b/serial-ros2/visual_studio/visual_studio.sln
diff --git a/vision_opencv.zip b/vision_opencv.zip
diff --git a/yacyac_api_server/.eslintrc.js b/yacyac_api_server/.eslintrc.js
diff --git a/yacyac_api_server/.gitignore b/yacyac_api_server/.gitignore
diff --git a/yacyac_api_server/.prettierrc b/yacyac_api_server/.prettierrc
diff --git a/yacyac_api_server/README.md b/yacyac_api_server/README.md
diff --git a/yacyac_api_server/nest-cli.json b/yacyac_api_server/nest-cli.json
diff --git a/yacyac_api_server/package.json b/yacyac_api_server/package.json
diff --git a/yacyac_api_server/src/app.controller.spec.ts b/yacyac_api_server/src/app.controller.spec.ts
diff --git a/yacyac_api_server/src/app.controller.ts b/yacyac_api_server/src/app.controller.ts
diff --git a/yacyac_api_server/src/app.module.ts b/yacyac_api_server/src/app.module.ts
diff --git a/yacyac_api_server/src/app.service.ts b/yacyac_api_server/src/app.service.ts
diff --git a/yacyac_api_server/src/historys/dto/create-history.dto.ts b/yacyac_api_server/src/historys/dto/create-history.dto.ts
diff --git a/yacyac_api_server/src/historys/dto/update-history.dto.ts b/yacyac_api_server/src/historys/dto/update-history.dto.ts
diff --git a/yacyac_api_server/src/historys/entities/history.entity.ts b/yacyac_api_server/src/historys/entities/history.entity.ts
diff --git a/yacyac_api_server/src/historys/historys.controller.ts b/yacyac_api_server/src/historys/historys.controller.ts
diff --git a/yacyac_api_server/src/historys/historys.module.ts b/yacyac_api_server/src/historys/historys.module.ts
diff --git a/yacyac_api_server/src/historys/historys.service.ts b/yacyac_api_server/src/historys/historys.service.ts
diff --git a/yacyac_api_server/src/main.ts b/yacyac_api_server/src/main.ts
diff --git a/yacyac_api_server/src/robots/dto/create-robot.dto.ts b/yacyac_api_server/src/robots/dto/create-robot.dto.ts
diff --git a/yacyac_api_server/src/robots/dto/supply-update.dto.ts b/yacyac_api_server/src/robots/dto/supply-update.dto.ts
diff --git a/yacyac_api_server/src/robots/dto/update-robot.dto.ts b/yacyac_api_server/src/robots/dto/update-robot.dto.ts
diff --git a/yacyac_api_server/src/robots/entities/robot.entity.ts b/yacyac_api_server/src/robots/entities/robot.entity.ts
diff --git a/yacyac_api_server/src/robots/robots.controller.ts b/yacyac_api_server/src/robots/robots.controller.ts
diff --git a/yacyac_api_server/src/robots/robots.module.ts b/yacyac_api_server/src/robots/robots.module.ts
diff --git a/yacyac_api_server/src/robots/robots.service.ts b/yacyac_api_server/src/robots/robots.service.ts
diff --git a/yacyac_api_server/src/users/dto/create-user.dto.ts b/yacyac_api_server/src/users/dto/create-user.dto.ts
diff --git a/yacyac_api_server/src/users/dto/update-user.dto.ts b/yacyac_api_server/src/users/dto/update-user.dto.ts
diff --git a/yacyac_api_server/src/users/entities/user.entity.ts b/yacyac_api_server/src/users/entities/user.entity.ts
diff --git a/yacyac_api_server/src/users/users.controller.ts b/yacyac_api_server/src/users/users.controller.ts
diff --git a/yacyac_api_server/src/users/users.module.ts b/yacyac_api_server/src/users/users.module.ts
diff --git a/yacyac_api_server/src/users/users.service.ts b/yacyac_api_server/src/users/users.service.ts
diff --git a/yacyac_api_server/test/app.e2e-spec.ts b/yacyac_api_server/test/app.e2e-spec.ts
diff --git a/yacyac_api_server/test/jest-e2e.json b/yacyac_api_server/test/jest-e2e.json
diff --git a/yacyac_api_server/tsconfig.build.json b/yacyac_api_server/tsconfig.build.json
diff --git a/yacyac_api_server/tsconfig.json b/yacyac_api_server/tsconfig.json
diff --git a/yacyac_api_server/yarn.lock b/yacyac_api_server/yarn.lock
diff --git a/yacyac_bringup/CMakeLists.txt b/yacyac_bringup/CMakeLists.txt
diff --git a/yacyac_bringup/README.md b/yacyac_bringup/README.md
diff --git a/yacyac_bringup/launch/robot_description.launch.py b/yacyac_bringup/launch/robot_description.launch.py
@@ -44,14 +44,14 @@ def generate_launch_description():
             "run",
             "tf2_ros",
             "static_transform_publisher",
+            "0.2",
             "0",
+            "0.18",
             "0",
-            "0.1",
-            "-3.141592",
             "0",
             "0",
             "base_link",
-            "laser",
+            "laser_frame",
         ],
         output="screen",
     )
@@ -85,6 +85,7 @@ def generate_launch_description():
             start_yacyac_servo_cmd,
 
             Node(
+                name="yacyac_io_node",
                 package="yacyac_io",
                 executable="tts",
                 output="screen",

diff --git a/yacyac_bringup/package.xml b/yacyac_bringup/package.xml
diff --git a/yacyac_camera/CMakeLists.txt b/yacyac_camera/CMakeLists.txt
diff --git a/yacyac_camera/include/yacyac_camera/camera_node.hpp b/yacyac_camera/include/yacyac_camera/camera_node.hpp
diff --git a/yacyac_camera/include/yacyac_camera/common.hpp b/yacyac_camera/include/yacyac_camera/common.hpp
diff --git a/yacyac_camera/include/yacyac_camera/qr_detector_node.hpp b/yacyac_camera/include/yacyac_camera/qr_detector_node.hpp
@@ -1,6 +1,8 @@
 #ifndef QR_DETECTOR_NODE_HPP_
 #define QR_DETECTOR_NODE_HPP_
+
 #include "yacyac_interface/msg/qrcode.hpp"
+#include "image_transport/image_transport.hpp"
 #include <cv_bridge/cv_bridge.h>
 #include <memory>
 #include <opencv2/opencv.hpp>
@@ -15,7 +17,10 @@ class QrDetectorNode final : public rclcpp::Node {
         RCLCPP_INFO(this->get_logger(), "QR DETECTOR NODE CREATE");
         scanner_.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
 
-        sub_ = this->create_subscription<sensor_msgs::msg::Image>(input_topic_name, rclcpp::SensorDataQoS(), std::bind(&QrDetectorNode::image_callback_, this, std::placeholders::_1));
+        image_transport::ImageTransport it(std::shared_ptr<QrDetectorNode>(this, [](auto *) {}));
+        sub_ = it.subscribe(input_topic_name, 1, std::bind(&QrDetectorNode::image_callback_, this, std::placeholders::_1));
+
+        // sub_ = this->create_subscription<sensor_msgs::msg::Image>(input_topic_name, rclcpp::SensorDataQoS(), );
 
         publisher_ = this->create_publisher<yacyac_interface::msg::Qrcode>(output, rclcpp::QoS(1));
 
@@ -31,18 +36,19 @@ class QrDetectorNode final : public rclcpp::Node {
         publisher_->publish(message);
     }
 
-    void image_callback_(sensor_msgs::msg::Image::UniquePtr msg)
+    void image_callback_(const sensor_msgs::msg::Image::ConstSharedPtr& msg)
     {
+        // RCLCPP_INFO(this->get_logger(), "IMAGE CALLBACK START");
         cv_bridge::CvImagePtr cv_ptr;
+
         try {
-
-            cv_ptr = cv_bridge::toCvCopy(std::move(msg), msg->encoding);
+            cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding);
         }
         catch (cv_bridge::Exception& e) {
             RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
             return;
         }
-
+        // RCLCPP_INFO(this->get_logger(), "IMAGE CALLBACK");
         qr_detect_infos_ = getQRcodeInfo(cv_ptr->image);
     }
 
@@ -66,7 +72,7 @@ class QrDetectorNode final : public rclcpp::Node {
         return qr_infos;
     };
     rclcpp::TimerBase::SharedPtr timer_;
-    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;
+    image_transport::Subscriber sub_;
     rclcpp::Publisher<yacyac_interface::msg::Qrcode>::SharedPtr publisher_;
 
     zbar::ImageScanner scanner_;

diff --git a/yacyac_camera/launch/camera.launch.py b/yacyac_camera/launch/camera.launch.py
diff --git a/yacyac_camera/package.xml b/yacyac_camera/package.xml
diff --git a/yacyac_camera/param/camera.yaml b/yacyac_camera/param/camera.yaml
diff --git a/yacyac_camera/src/camera_node.cpp b/yacyac_camera/src/camera_node.cpp
@@ -7,7 +7,7 @@ int main(int argc, char** argv)
 
     std::shared_ptr<CameraNode> camera_node = nullptr;
     try {
-        camera_node = std::make_shared<CameraNode>("/yacyac_camera", "camera_node", "/dev/video0");
+        camera_node = std::make_shared<CameraNode>("/yacyac_top_camera", "yacyac_top_camera", "/dev/video1");
     }
     catch (const std::exception& e) {
         fprintf(stderr, "%s Exiting..\n", e.what());

diff --git a/yacyac_camera/src/camera_qr_pipeline.cpp b/yacyac_camera/src/camera_qr_pipeline.cpp
@@ -41,4 +41,4 @@ int main(int argc, char* argv[])
     rclcpp::shutdown();
 
     return 0;
-}
+}
diff --git a/yacyac_camera/src/qr_detector_node.cpp b/yacyac_camera/src/qr_detector_node.cpp
@@ -7,7 +7,7 @@ int main(int argc, char** argv)
 
     std::shared_ptr<QrDetectorNode> qr_detector_node = nullptr;
     try {
-        qr_detector_node = std::make_shared<QrDetectorNode>("/qr_code", "/yacyac_camera", "qr_detector_node");
+        qr_detector_node = std::make_shared<QrDetectorNode>("yacyac_qr_detector", "/yacyac_top_camera", "/qr_node");
     }
     catch (const std::exception& e) {
         fprintf(stderr, "%s Exiting..\n", e.what());

diff --git a/yacyac_cartographer/CMakeLists.txt b/yacyac_cartographer/CMakeLists.txt
diff --git a/yacyac_cartographer/README.md b/yacyac_cartographer/README.md
diff --git a/yacyac_cartographer/configuration/cartographer_2d.lua b/yacyac_cartographer/configuration/cartographer_2d.lua
@@ -43,4 +43,4 @@ POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
 
 -- POSE_GRAPH.optimize_every_n_nodes = 0
 
-return options
+return options
diff --git a/yacyac_cartographer/configuration/mapping_rviz.rviz b/yacyac_cartographer/configuration/mapping_rviz.rviz
diff --git a/yacyac_cartographer/launch/mapping.launch.py b/yacyac_cartographer/launch/mapping.launch.py
diff --git a/yacyac_cartographer/launch/occupancy_grid.launch.py b/yacyac_cartographer/launch/occupancy_grid.launch.py
diff --git a/yacyac_cartographer/package.xml b/yacyac_cartographer/package.xml
diff --git a/yacyac_core/CMakeLists.txt b/yacyac_core/CMakeLists.txt
diff --git a/yacyac_core/bt_xml/bt_nav_mememan.xml b/yacyac_core/bt_xml/bt_nav_mememan.xml
diff --git a/yacyac_core/bt_xml/bt_nav_yac_supply copy.xml b/yacyac_core/bt_xml/bt_nav_yac_supply copy.xml
diff --git a/yacyac_core/bt_xml/bt_nav_yac_supply.xml b/yacyac_core/bt_xml/bt_nav_yac_supply.xml
@@ -36,12 +36,14 @@
             </ForceSuccess>
 
             <ForceSuccess>
-              <Sequence>
-                <!-- <Nav2Client goal="{Goal_b}"/> -->
-                <QRClient />
-                <SupplyAcceptClient />
-                <YacSupplyCilent goal="{yac_b}"/>
-              </Sequence>
+              <Delay delay_msec="10000">
+                <Sequence>
+                  <!-- <Nav2Client goal="{Goal_b}"/> -->
+                  <QRClient />
+                  <SupplyAcceptClient />
+                  <YacSupplyCilent goal="{yac_b}"/>
+                </Sequence>
+              </Delay>
             </ForceSuccess>
 
             <ForceSuccess>

diff --git a/yacyac_core/bt_xml/bt_yac_supply.xml b/yacyac_core/bt_xml/bt_yac_supply.xml
diff --git a/yacyac_core/bt_xml/default.xml b/yacyac_core/bt_xml/default.xml
diff --git a/yacyac_core/include/yacyac_core/message.hpp b/yacyac_core/include/yacyac_core/message.hpp
diff --git a/yacyac_core/include/yacyac_core/qr_client.hpp b/yacyac_core/include/yacyac_core/qr_client.hpp
@@ -6,7 +6,7 @@
 #include <std_msgs/msg/int8.hpp>
 
 #include "yacyac_interface/msg/qrcode.hpp"
-#include "yacyac_interface/srv/tts.hpp"
+#include "yacyac_interface/srv/tts.hpp" 
 
 class QRClient : public BT::StatefulActionNode {
 public:

diff --git a/yacyac_core/include/yacyac_core/supply_accept_client.hpp b/yacyac_core/include/yacyac_core/supply_accept_client.hpp
diff --git a/yacyac_core/launch/core.launch.py b/yacyac_core/launch/core.launch.py
diff --git a/yacyac_core/package.xml b/yacyac_core/package.xml
diff --git a/yacyac_core/script/test.py b/yacyac_core/script/test.py
diff --git a/yacyac_core/src/bt_ros2.cpp b/yacyac_core/src/bt_ros2.cpp
diff --git a/yacyac_core/src/main.cpp b/yacyac_core/src/main.cpp
diff --git a/yacyac_core/src/message.cpp b/yacyac_core/src/message.cpp
diff --git a/yacyac_core/src/nav2_client.cpp b/yacyac_core/src/nav2_client.cpp
diff --git a/yacyac_core/src/qr_client.cpp b/yacyac_core/src/qr_client.cpp
@@ -13,7 +13,7 @@ BT::NodeStatus QRClient::onStart()
 
     const std::string QR_topic_name = "/qr_node";
     const std::string mode_topic_name = "/mode";
-    const std::string TTS_service_name = "/yacyac/io";
+    const std::string TTS_service_name = "/yacyac/io"; 
 
     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));

diff --git a/yacyac_core/src/send_goal.cpp b/yacyac_core/src/send_goal.cpp
diff --git a/yacyac_core/src/send_goal3.cpp b/yacyac_core/src/send_goal3.cpp
diff --git a/yacyac_core/src/supply_accept_client.cpp b/yacyac_core/src/supply_accept_client.cpp
diff --git a/yacyac_core/src/yac_supply_client.cpp b/yacyac_core/src/yac_supply_client.cpp
diff --git a/yacyac_interface/CMakeLists.txt b/yacyac_interface/CMakeLists.txt
diff --git a/yacyac_interface/action/Supply.action b/yacyac_interface/action/Supply.action
diff --git a/yacyac_interface/msg/Pose.msg b/yacyac_interface/msg/Pose.msg
diff --git a/yacyac_interface/msg/Qrcode.msg b/yacyac_interface/msg/Qrcode.msg
diff --git a/yacyac_interface/msg/Servo.msg b/yacyac_interface/msg/Servo.msg
diff --git a/yacyac_interface/package.xml b/yacyac_interface/package.xml
diff --git a/yacyac_interface/srv/TTS.srv b/yacyac_interface/srv/TTS.srv
diff --git a/yacyac_io/package.xml b/yacyac_io/package.xml
diff --git a/yacyac_io/resource/yacyac_io b/yacyac_io/resource/yacyac_io
diff --git a/yacyac_io/setup.cfg b/yacyac_io/setup.cfg
diff --git a/yacyac_io/setup.py b/yacyac_io/setup.py
diff --git a/yacyac_monitor/README.md b/yacyac_monitor/README.md
diff --git a/yacyac_monitor/package.json b/yacyac_monitor/package.json
diff --git a/yacyac_monitor/public/favicon.ico b/yacyac_monitor/public/favicon.ico
diff --git a/yacyac_monitor/public/index.html b/yacyac_monitor/public/index.html
diff --git a/yacyac_monitor/public/logo192.png b/yacyac_monitor/public/logo192.png
diff --git a/yacyac_monitor/public/logo512.png b/yacyac_monitor/public/logo512.png
diff --git a/yacyac_monitor/public/manifest.json b/yacyac_monitor/public/manifest.json
diff --git a/yacyac_monitor/public/one.png b/yacyac_monitor/public/one.png
diff --git a/yacyac_monitor/public/robot.glb b/yacyac_monitor/public/robot.glb
diff --git a/yacyac_monitor/public/robots.txt b/yacyac_monitor/public/robots.txt
diff --git a/yacyac_monitor/public/scene.bin b/yacyac_monitor/public/scene.bin
diff --git a/yacyac_monitor/public/scene.gltf b/yacyac_monitor/public/scene.gltf
diff --git a/yacyac_monitor/src/App.css b/yacyac_monitor/src/App.css
diff --git a/yacyac_monitor/src/App.test.tsx b/yacyac_monitor/src/App.test.tsx
diff --git a/yacyac_monitor/src/App.tsx b/yacyac_monitor/src/App.tsx
diff --git a/yacyac_monitor/src/apis/History.ts b/yacyac_monitor/src/apis/History.ts
diff --git a/yacyac_monitor/src/apis/fetcher.ts b/yacyac_monitor/src/apis/fetcher.ts
diff --git a/yacyac_monitor/src/assets/configs/map.json b/yacyac_monitor/src/assets/configs/map.json
diff --git a/yacyac_monitor/src/components/Clock/index.tsx b/yacyac_monitor/src/components/Clock/index.tsx
diff --git a/yacyac_monitor/src/components/RobotMap/index.tsx b/yacyac_monitor/src/components/RobotMap/index.tsx
diff --git a/yacyac_monitor/src/components/RobotMap/styled.tsx b/yacyac_monitor/src/components/RobotMap/styled.tsx
diff --git a/yacyac_monitor/src/components/StatusCard/index.tsx b/yacyac_monitor/src/components/StatusCard/index.tsx
diff --git a/yacyac_monitor/src/components/StatusCard/styled.tsx b/yacyac_monitor/src/components/StatusCard/styled.tsx
diff --git a/yacyac_monitor/src/index.css b/yacyac_monitor/src/index.css
diff --git a/yacyac_monitor/src/index.tsx b/yacyac_monitor/src/index.tsx
diff --git a/yacyac_monitor/src/logo.svg b/yacyac_monitor/src/logo.svg
diff --git a/yacyac_monitor/src/lotties/eyes.json b/yacyac_monitor/src/lotties/eyes.json
diff --git a/yacyac_monitor/src/lotties/pill.json b/yacyac_monitor/src/lotties/pill.json
diff --git a/yacyac_monitor/src/pages/Containers.tsx b/yacyac_monitor/src/pages/Containers.tsx
diff --git a/yacyac_monitor/src/pages/Dashboard.tsx b/yacyac_monitor/src/pages/Dashboard.tsx
diff --git a/yacyac_monitor/src/pages/Display.tsx b/yacyac_monitor/src/pages/Display.tsx
diff --git a/yacyac_monitor/src/pages/Historys.tsx b/yacyac_monitor/src/pages/Historys.tsx
diff --git a/yacyac_monitor/src/react-app-env.d.ts b/yacyac_monitor/src/react-app-env.d.ts
diff --git a/yacyac_monitor/src/reportWebVitals.ts b/yacyac_monitor/src/reportWebVitals.ts
diff --git a/yacyac_monitor/src/setupTests.ts b/yacyac_monitor/src/setupTests.ts
diff --git a/yacyac_monitor/src/templates/AppTemplate.tsx b/yacyac_monitor/src/templates/AppTemplate.tsx
diff --git a/yacyac_monitor/src/types/index.ts b/yacyac_monitor/src/types/index.ts
diff --git a/yacyac_monitor/src/utils/math.ts b/yacyac_monitor/src/utils/math.ts
diff --git a/yacyac_monitor/tsconfig.json b/yacyac_monitor/tsconfig.json
diff --git a/yacyac_monitor/yarn-error.log b/yacyac_monitor/yarn-error.log
diff --git a/yacyac_monitor/yarn.lock b/yacyac_monitor/yarn.lock
diff --git a/yacyac_navigation2/CMakeLists.txt b/yacyac_navigation2/CMakeLists.txt
diff --git a/yacyac_navigation2/README.md b/yacyac_navigation2/README.md
diff --git a/yacyac_navigation2/launch/navigation.launch.py b/yacyac_navigation2/launch/navigation.launch.py
@@ -30,7 +30,7 @@ def generate_launch_description():
     pkg_path = os.path.join(get_package_share_directory(package_name))
 
     use_sim_time = LaunchConfiguration("use_sim_time", default="false")
-    map_dir = LaunchConfiguration("map", default=os.path.join(pkg_path, "map", "one.yaml"))
+    map_dir = LaunchConfiguration("map", default=os.path.join(pkg_path, "map", "test.yaml"))
 
     param_file_name = "yacyac.yaml"
     param_dir = LaunchConfiguration(

diff --git a/yacyac_navigation2/map/halfhalf5.pgm b/yacyac_navigation2/map/halfhalf5.pgm
diff --git a/yacyac_navigation2/map/halfhalf5.yaml b/yacyac_navigation2/map/halfhalf5.yaml
diff --git a/yacyac_navigation2/map/halfhalf5_org.pgm b/yacyac_navigation2/map/halfhalf5_org.pgm
diff --git a/yacyac_navigation2/map/map.pgm b/yacyac_navigation2/map/map.pgm
diff --git a/yacyac_navigation2/map/map.yaml b/yacyac_navigation2/map/map.yaml
@@ -0,0 +1,7 @@
+image: map.pgm
+mode: trinary
+resolution: 0.05
+origin: [-17.3, -9.82, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25
diff --git a/yacyac_navigation2/map/one.pgm b/yacyac_navigation2/map/one.pgm
diff --git a/yacyac_navigation2/map/one.yaml b/yacyac_navigation2/map/one.yaml
diff --git a/yacyac_navigation2/map/one3.pgm b/yacyac_navigation2/map/one3.pgm
diff --git a/yacyac_navigation2/map/test.pgm b/yacyac_navigation2/map/test.pgm
diff --git a/yacyac_navigation2/map/test.yaml b/yacyac_navigation2/map/test.yaml
@@ -1,7 +1,7 @@
 image: test.pgm
 mode: trinary
 resolution: 0.05
-origin: [-3.65, -3.08, 0]
+origin: [-5.48, -15.3, 0]
 negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.25
diff --git a/yacyac_navigation2/package.xml b/yacyac_navigation2/package.xml
diff --git a/yacyac_navigation2/param/yacyac.yaml b/yacyac_navigation2/param/yacyac.yaml
@@ -14,8 +14,8 @@ amcl:
     global_frame_id: "map"
     lambda_short: 0.1
     laser_likelihood_max_dist: 2.0
-    laser_max_range: 12.0
-    laser_min_range: -1.0
+    laser_max_range: 14.0
+    laser_min_range: 0.03
     laser_model_type: "likelihood_field"
     max_beams: 60
     max_particles: 2000
@@ -126,11 +126,11 @@ controller_server:
       debug_trajectory_details: True
       min_vel_x: 0.0
       min_vel_y: 0.0
-      max_vel_x: 0.4
+      max_vel_x: 0.15
       max_vel_y: 0.0
-      max_vel_theta: 1.0
+      max_vel_theta: 0.8
       min_speed_xy: 0.0
-      max_speed_xy: 0.4
+      max_speed_xy: 0.2
       min_speed_theta: 0.0
       acc_lim_x: 2.5
       acc_lim_y: 0.0
@@ -145,7 +145,7 @@ controller_server:
       linear_granularity: 0.05
       angular_granularity: 0.025
       transform_tolerance: 0.2
-      xy_goal_tolerance: 0.25
+      xy_goal_tolerance: 0.2
       trans_stopped_velocity: 0.25
       short_circuit_trajectory_evaluation: True
       stateful: True
@@ -192,6 +192,8 @@ local_costmap:
           max_obstacle_height: 2.0
           clearing: True
           marking: True
+          raytrace_min_range: 0.3
+          obstacle_min_range: 0.3
           data_type: "LaserScan"
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
@@ -210,9 +212,9 @@ local_costmap:
           marking: True
           data_type: "LaserScan"
           raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
+          raytrace_min_range: 0.3
           obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
+          obstacle_min_range: 0.3
       static_layer:
         map_subscribe_transient_local: True
       always_send_full_costmap: True
@@ -230,7 +232,7 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      use_sim_time: True
+      use_sim_time: False
       robot_radius: 0.233
       resolution: 0.05
       track_unknown_space: true
@@ -246,9 +248,9 @@ global_costmap:
           marking: True
           data_type: "LaserScan"
           raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
+          raytrace_min_range: 0.3
           obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
+          obstacle_min_range: 0.3
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -287,7 +289,7 @@ global_costmap:
 map_server:
   ros__parameters:
     use_sim_time: False
-    yaml_filename: "one.yaml"
+    yaml_filename: "test.yaml"
 
 map_saver:
   ros__parameters:
@@ -327,10 +329,10 @@ recoveries_server:
     global_frame: odom
     robot_base_frame: base_link
     transform_timeout: 0.1
-    use_sim_time: true
+    use_sim_time: False
     simulate_ahead_time: 2.0
-    max_rotational_vel: 1.0
-    min_rotational_vel: 0.4
+    max_rotational_vel: 0.5
+    min_rotational_vel: 0.2
     rotational_acc_lim: 3.2
 
 robot_state_publisher:
@@ -339,6 +341,7 @@ robot_state_publisher:
 
 waypoint_follower:
   ros__parameters:
+    use_sim_time: False
     loop_rate: 2000
     stop_on_failure: false
     waypoint_task_executor_plugin: "wait_at_waypoint"   

diff --git a/yacyac_servo/launch/yacyac_servo.launch.py b/yacyac_servo/launch/yacyac_servo.launch.py
diff --git a/yacyac_servo/package.xml b/yacyac_servo/package.xml
diff --git a/yacyac_servo/resource/yacyac_servo b/yacyac_servo/resource/yacyac_servo
diff --git a/yacyac_servo/setup.cfg b/yacyac_servo/setup.cfg
diff --git a/yacyac_servo/setup.py b/yacyac_servo/setup.py
diff --git a/yacyac_servo/test/test_copyright.py b/yacyac_servo/test/test_copyright.py
diff --git a/yacyac_servo/test/test_flake8.py b/yacyac_servo/test/test_flake8.py
diff --git a/yacyac_servo/test/test_pep257.py b/yacyac_servo/test/test_pep257.py
diff --git a/yacyac_servo/yacyac_servo/__init__.py b/yacyac_servo/yacyac_servo/__init__.py
diff --git a/yacyac_servo/yacyac_servo/action_servo_ctrl.py b/yacyac_servo/yacyac_servo/action_servo_ctrl.py
diff --git a/yacyac_servo/yacyac_servo/servo_ctrl.py b/yacyac_servo/yacyac_servo/servo_ctrl.py