add wust typr mpc and mutipule x

2026-03-27 03:41:42 +08:00
parent 2c64655fae
commit 7dcb53bb77
192 changed files with 29571 additions and 9 deletions
--- a/wust_vision-main/test/nav.cpp
+++ b/wust_vision-main/test/nav.cpp
@@ -0,0 +1,67 @@
+#include "geometry_msgs/msg/twist.hpp"
+#include "ros2/ros2.hpp"
+#include "tasks/packet_typedef.hpp"
+#include <rclcpp/subscription.hpp>
+#include <wust_vl/common/drivers/serial_driver.hpp>
+#include <wust_vl/common/utils/logger.hpp>
+#include <wust_vl/common/utils/timer.hpp>
+using namespace wust_vision;
+class Nav: public rclcpp::Node {
+public:
+    Nav(): Node("wust_vision_global_node") {
+        cmd_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
+            "cmd_vel",
+            10,
+            std::bind(&Nav::twistCb, this, std::placeholders::_1)
+        );
+        serial_ = std::make_shared<wust_vl::common::drivers::SerialDriver>();
+        wust_vl::common::drivers::SerialDriver::SerialPortConfig cfg {
+            /*baud*/ 115200,
+            /*csize*/ 8,
+            boost::asio::serial_port_base::parity::none,
+            boost::asio::serial_port_base::stop_bits::one,
+            boost::asio::serial_port_base::flow_control::none
+        };
+        serial_->init_port("/dev/ttyACM0", cfg);
+        serial_->set_receive_callback(std::bind(
+            &Nav::serialCallback,
+            this,
+            std::placeholders::_1,
+            std::placeholders::_2
+
+        ));
+        serial_->set_error_callback([&](const boost::system::error_code& ec) {
+            WUST_ERROR("serial") << "serial error: " << ec.message();
+        });
+    }
+    void serialCallback(const uint8_t* data, std::size_t len) {}
+    void twistCb(const geometry_msgs::msg::Twist::SharedPtr msg) {
+        NavRobotCmdData send_data;
+        send_data.cmd_ID = ID_NAV_CMD;
+        send_data.packet_type = ID_NAV_CONTROL;
+        send_data.time_stamp =
+            static_cast<uint32_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
+                                      wust_vl::common::utils::time_utils::now().time_since_epoch()
+            )
+                                      .count());
+        send_data.vx = -msg->linear.y;
+        send_data.vy = msg->linear.x;
+        send_data.wz = msg->angular.z;
+        if (serial_) {
+            serial_->write(std::move(wust_vl::common::drivers::toVector(send_data)));
+        }
+    }
+    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
+    std::shared_ptr<wust_vl::common::drivers::SerialDriver> serial_;
+};
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+
+    {
+        auto node = rclcpp::Node::make_shared("wust_vision_global_node");
+        rclcpp::spin(node);
+        // rclcpp::shutdown();
+    }
+
+    return 0;
+}