Sentry following mode

Meta-Team · Jan 5, 2025 · 1497705 · 1497705
1 parent b606ad3
commit 1497705
Show file tree

Hide file tree

Showing 4 changed files with 26 additions and 3 deletions.
diff --git a/decision/dbus_vehicle/include/dbus_vehicle/dbus_interpreter.h b/decision/dbus_vehicle/include/dbus_vehicle/dbus_interpreter.h
@@ -9,14 +9,17 @@
 #include "behavior_interface/msg/move.hpp"
 #include "behavior_interface/msg/shoot.hpp"
 #include "behavior_interface/msg/aim.hpp"
+#include "behavior_interface/msg/chassis.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include <behavior_interface/msg/detail/chassis__struct.hpp>
 
 #define PERIOD 10 // ms
 
 using operation_interface::msg::DbusControl;
 using behavior_interface::msg::Move;
 using behavior_interface::msg::Shoot;
 using behavior_interface::msg::Aim;
+using behavior_interface::msg::Chassis;
 
 class DbusInterpreter
 {
@@ -34,6 +37,8 @@ class DbusInterpreter
 
     Aim::SharedPtr get_aim() const;
 
+    Chassis::SharedPtr get_chassis() const;
+
     bool is_active() const { return active; }
 
 private:
@@ -49,6 +54,7 @@ class DbusInterpreter
     Move::SharedPtr move_;
     Shoot::SharedPtr shoot_;
     Aim::SharedPtr aim_;
+    Chassis::SharedPtr chassis_;
 
     void update();
 

diff --git a/decision/dbus_vehicle/src/dbus_interpreter.cpp b/decision/dbus_vehicle/src/dbus_interpreter.cpp
@@ -9,10 +9,11 @@ DbusInterpreter::DbusInterpreter(double max_vel, double max_omega, double aim_se
     ls_x = ls_y = rs_x = rs_y = wheel = 0;
     lsw = rsw = "";
 
-    // initialize move, shoot, and aim
+    // initialize move, shoot, aim, and chassis state
     move_ = std::make_shared<Move>();
     shoot_ = std::make_shared<Shoot>();
     aim_ = std::make_shared<Aim>();
+    chassis_ = std::make_shared<Chassis>();
 
     // initialize update thread
     update_thread = std::thread([this](){
@@ -50,9 +51,16 @@ void DbusInterpreter::update()
 
     move_->vel_x = max_vel * ls_x;
     move_->vel_y = max_vel * ls_y;
-    move_->omega = max_omega * wheel;
     aim_->pitch += aim_sens * rs_x * PERIOD / 1000; curb(aim_->pitch, M_PI_4);
+    move_->omega = max_omega * wheel;
     aim_->yaw += aim_sens * rs_y * PERIOD / 1000;
+    if(wheel > 0.01){      
+        chassis_->mode = behavior_interface::msg::Chassis::CHASSIS;
+    }else{
+        chassis_->mode = behavior_interface::msg::Chassis::CHASSIS_FOLLOW;
+    }
+
+
 
     if (rsw == "UP")
     {
@@ -106,6 +114,11 @@ Aim::SharedPtr DbusInterpreter::get_aim() const
     return aim_;
 }
 
+Chassis::SharedPtr DbusInterpreter::get_chassis() const
+{
+    return chassis_;
+}
+
 void DbusInterpreter::curb(double &val, double max_val)
 {
     if (val > max_val)

diff --git a/decision/dbus_vehicle/src/dbus_vehicle.cpp b/decision/dbus_vehicle/src/dbus_vehicle.cpp
@@ -17,6 +17,7 @@ class DbusVehicle : public rclcpp::Node
         double deadzone = this->declare_parameter("control.deadzone", 0.05);
         std::string aim_topic = this->declare_parameter("aim_topic", "aim");
         std::string shoot_topic = this->declare_parameter("shoot_topic", "shoot");
+        std::string chassis_topic = this->declare_parameter("chassis_topic","chassis_cmd");
         RCLCPP_INFO(this->get_logger(), "max_vel: %f, max_omega: %f, aim_sens: %f, deadzone: %f",
             max_vel, max_omega, aim_sens, deadzone);
         enable_ros2_control_ = this->declare_parameter("enable_ros2_control", false);
@@ -32,6 +33,7 @@ class DbusVehicle : public rclcpp::Node
         }
         shoot_pub_ = this->create_publisher<behavior_interface::msg::Shoot>(shoot_topic, 10);
         aim_pub_ = this->create_publisher<behavior_interface::msg::Aim>(aim_topic, 10);
+        chassis_pub_ = this->create_publisher<behavior_interface::msg::Chassis>(chassis_topic, 10);
         dbus_sub_ = this->create_subscription<operation_interface::msg::DbusControl>(
             "dbus_control", 10,
             std::bind(&DbusVehicle::dbus_callback, this, std::placeholders::_1));
@@ -52,6 +54,7 @@ class DbusVehicle : public rclcpp::Node
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr move_pub_ros2_control_;
     rclcpp::Publisher<behavior_interface::msg::Shoot>::SharedPtr shoot_pub_;
     rclcpp::Publisher<behavior_interface::msg::Aim>::SharedPtr aim_pub_;
+    rclcpp::Publisher<behavior_interface::msg::Chassis>::SharedPtr chassis_pub_;
     rclcpp::TimerBase::SharedPtr timer_;
 
     bool enable_ros2_control_;
@@ -71,6 +74,7 @@ class DbusVehicle : public rclcpp::Node
         }
         shoot_pub_->publish(*interpreter_->get_shoot());
         aim_pub_->publish(*interpreter_->get_aim());
+        chassis_pub_->publish(*interpreter_->get_chassis());
     }
 };
 #include "rclcpp_components/register_node_macro.hpp"

diff --git a/decomposition/meta_chassis_controller/src/omni_chassis_controller.cpp b/decomposition/meta_chassis_controller/src/omni_chassis_controller.cpp
@@ -78,7 +78,7 @@ OmniChassisController::on_configure(const rclcpp_lifecycle::State & /*previous_s
 
     subscribers_qos = rclcpp::SystemDefaultsQoS();
     subscribers_qos.reliable();
-    subscribers_qos.transient_local();
+    // subscribers_qos.transient_local();
 
     chassis_sub_ = get_node()->create_subscription<behavior_interface::msg::Chassis>(
         "/chassis_cmd", subscribers_qos,