recharge

src/wheelchair_state_machine/src/Untitled-1.cpp

@@ -1,7 +0,0 @@
- // 更新中间点
-    result.middle_point = IndexPoint(x, y, 0);
- // 计算角平分线
-    auto [k3, b3] = RechargeTool::calculateAngleBisector(k1, b1, k2, b2);
-    current_angle_bisector_k_ = k3;
-    current_angle_bisector_b_ = b3;
-    has_angle_bisector_ = true;

src/wheelchair_state_machine/src/lidascan_ctrl.cpp

@@ -465,16 +465,16 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         double adjust_angular = 0.0;
         std::string adjust_type;
 
-        const double ANGLE_8DEG = 5.0 * M_PI / 180.0; // 6度阈值
-        const double DY_THRESHOLD = 0.05;             // 5cm阈值
+        const double ANGLE_8DEG = 5.0 * M_PI / 180.0; // 5度阈值
+       // const double DY_THRESHOLD = 0.05;             // 5cm阈值
 
         // 优先调整角度
         if (fabs(yaw_diff) > ANGLE_8DEG)
         {
             adjust_type = "角度调整";
             // 根据夹角大小计算调整时间
-            adjust_duration = fabs(yaw_diff) / 0.35; // 角速度0.25 rad/s
-            adjust_angular = (yaw_diff > 0) ? -0.35 : 0.35;
+            adjust_duration = fabs(yaw_diff) / 0.15; // 角速度0.25 rad/s
+            adjust_angular = (yaw_diff > 0) ? -0.15 : 0.15;
 
             RCLCPP_WARN(node_->get_logger(), "旋转 %.2f 秒", adjust_duration);
         }
@@ -520,7 +520,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
 
             // 5. 向前走一小段距离（新增功能）
             const double FORWARD_DISTANCE = 0.80; // 向前走150cm
-            const double FORWARD_SPEED = 0.08;    // 前进速度 0.08 m/s
+            const double FORWARD_SPEED = 0.09;    // 前进速度 0.08 m/s
 
             // 计算前进所需时间
             double forward_duration = FORWARD_DISTANCE / FORWARD_SPEED;

src/wheelchair_state_machine/src/rotation_manager.cpp

@@ -5,7 +5,7 @@
 using namespace std::chrono_literals;
 
 RotationManager::RotationManager(rclcpp::Node *node)
-    : node_(node), is_rotating_(false), rotation_timeout_(40.0) // 旋转20秒超时
+    : node_(node), is_rotating_(false), rotation_timeout_(60.0) // 旋转20秒超时
       ,
       angular_velocity_(0.15) // 角速度0.3 rad/s
       ,

src/wheelchair_state_machine/src/workflow.cpp

@@ -125,7 +125,7 @@ void WorkflowManager::startRotationDelayTimer()
 
         // 创建0.5秒延迟定时器
         rotation_delay_timer_ = node_->create_wall_timer(
-            std::chrono::milliseconds(2000), 
+            std::chrono::milliseconds(1500), 
             [this]()
             {
                 this->rotationDelayCallback();

src/wheelchair_state_machine/src/第一种调到垂线范围（角度误差在5度）

@@ -1,2 +1,13 @@
 第一种调到垂线范围（角度误差在5度）
 第二种种调到比垂线角度过大（角度误差比5度要大）
+
+
+录雷达
+/scan/back
+/scan/out
+/scan/out_line
+/cmd_vel_raw
+/cmd_vel
+/cmd_vel_nav
+/wheel/twist
+