wheelchair_state_machine

src/wheelchair_state_machine/src/lidascan_ctrl.cpp

@@ -212,7 +212,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
     auto sections = getSections(begin_index, end_index, msg, xy_list);
     if (sections.empty())
     {
-        //RCLCPP_WARN(node_->get_logger(), "没有获取到分段");
+        // RCLCPP_WARN(node_->get_logger(), "没有获取到分段");
         return false;
     }
 
@@ -389,7 +389,6 @@ void LidarScanController::procData(RechargeResult &result)
     // RCLCPP_INFO(node_->get_logger(), "右直线角度: %.2f°", right_angle);
     // RCLCPP_INFO(node_->get_logger(), "角平分线角度: %.2f°", bisector_angle);
 
-
     // 更新结果
     result.middle_line = StraightLine(k3, b3);
     result.left_line = StraightLine(k1, b1);
@@ -412,12 +411,12 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         // 清除控制命令，避免定时器发布旧命令
         // 修改：不再清除控制命令，而是继续旋转
         // 只清除线性速度，保持角速度
-        double angular_velocity = 0.15;  // 与RotationManager中的角速度一致
-        
-        ctrl_twist_.linear.x = 0.0;       // 线速度保持0
-        ctrl_twist_.angular.z = angular_velocity;  // 保持旋转速度
+        double angular_velocity = 0.15; // 与RotationManager中的角速度一致
+
+        ctrl_twist_.linear.x = 0.0;               // 线速度保持0
+        ctrl_twist_.angular.z = angular_velocity; // 保持旋转速度
         ctrl_twist_time_ = node_->now();
-        
+
         // 发布旋转命令
         pub_ctrl_->publish(ctrl_twist_);
         return;
@@ -445,20 +444,131 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
     // 计算偏航角误差
     double yaw_diff = atan(k);
     RCLCPP_INFO(node_->get_logger(), "偏航角误差: %.4f rad (%.2f 度)",
-                 yaw_diff, yaw_diff * 180.0 / M_PI);
+                yaw_diff, yaw_diff * 180.0 / M_PI);
+
+    if (checkStationAlignment(dx, dy, yaw_diff))
+    {
+        // 停止行车中，准备向前移动，再校准回充
+        RCLCPP_WARN(node_->get_logger(), "===== 开始对准误差调整 =====");
+        RCLCPP_WARN(node_->get_logger(), "当前误差 - 夹角:%.2f°, dy:%.3fm",
+                    yaw_diff * 180.0 / M_PI, dy);
+
+        // 1. 停止当前运动
+        ctrl_twist_.linear.x = 0.0;
+        ctrl_twist_.angular.z = 0.0;
+        ctrl_twist_time_ = node_->now();
+        pub_ctrl_->publish(ctrl_twist_);
+        rclcpp::sleep_for(std::chrono::milliseconds(300));
+
+        // 2. 根据误差类型进行旋转调整
+        double adjust_duration = 0.0;
+        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阈值
+
+        // 优先调整角度
+        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;
+
+            RCLCPP_WARN(node_->get_logger(), "旋转 %.2f 秒", adjust_duration);
+        }
+        // 角度满足要求后调整dy
+        // else
+        // {
+        //     adjust_type = "横向偏移调整";
+        //     // dy为正(偏右) -> 向左转；dy为负(偏左) -> 向右转
+        //     adjust_duration = fabs(dy) * 20.0;  // 每5cm误差旋转1秒
+        //     adjust_angular = (dy > 0) ? 0.10 : -0.10;
+
+        //     RCLCPP_WARN(node_->get_logger(), "旋转 %.2f 秒",adjust_duration);
+        // }
+
+        // 3. 执行旋转调整
+        if (active_)
+        {
+            if (adjust_duration > 0.1)
+            {
+                geometry_msgs::msg::Twist adjust_cmd;
+                adjust_cmd.linear.x = 0.0;
+                adjust_cmd.angular.z = adjust_angular;
+
+                // 按计算的时间连续旋转
+                rclcpp::Time adjust_start = node_->now();
+                int publish_count = 0;
+
+                while ((node_->now() - adjust_start).seconds() < adjust_duration)
+                {
+                    pub_ctrl_->publish(adjust_cmd);
+                    publish_count++;
+                    rclcpp::sleep_for(std::chrono::milliseconds(50));
+                }
 
+                RCLCPP_WARN(node_->get_logger(), "%s完成: 发布%d次命令, 实际旋转%.2f秒",
+                            adjust_type.c_str(), publish_count, adjust_duration);
+
+                // 4. 停止旋转
+                adjust_cmd.angular.z = 0.0;
+                pub_ctrl_->publish(adjust_cmd);
+                rclcpp::sleep_for(std::chrono::milliseconds(500));
+            }
+
+            // 5. 向前走一小段距离（新增功能）
+            const double FORWARD_DISTANCE = 0.80; // 向前走150cm
+            const double FORWARD_SPEED = 0.08;    // 前进速度 0.08 m/s
+
+            // 计算前进所需时间
+            double forward_duration = FORWARD_DISTANCE / FORWARD_SPEED;
+
+            RCLCPP_WARN(node_->get_logger(), "开始向前调整: 距离=%.2fm, 速度=%.2fm/s, 时间=%.2fs",
+                        FORWARD_DISTANCE, FORWARD_SPEED, forward_duration);
+
+            // 发送前进命令
+            geometry_msgs::msg::Twist forward_cmd;
+            forward_cmd.linear.x = FORWARD_SPEED;
+            forward_cmd.angular.z = 0.0;
+
+            rclcpp::Time forward_start = node_->now();
+            int forward_count = 0;
+
+            while ((node_->now() - forward_start).seconds() < forward_duration)
+            {
+                pub_ctrl_->publish(forward_cmd);
+                forward_count++;
+                rclcpp::sleep_for(std::chrono::milliseconds(50));
+            }
+
+            // 停止前进
+            forward_cmd.linear.x = 0.0;
+            pub_ctrl_->publish(forward_cmd);
+
+            RCLCPP_INFO(node_->get_logger(), "前进调整完成: 发布%d次命令, 实际前进%.2f秒",
+                        forward_count, forward_duration);
+        }
+
+        // 6. 重要：等待新一帧激光数据并更新result
+        // 等待激光数据更新（约100ms足够接收新数据）
+        rclcpp::sleep_for(std::chrono::milliseconds(450));
+
+        // 注意：这里无法直接更新result，但函数返回后，下一帧激光数据会触发新的processSegmentData
+        // 从而得到新的result，重新进入controlRechargeMotion
+
+        RCLCPP_WARN(node_->get_logger(), "===== 对准误差调整完成 =====");
+
+        // 7. 返回，等待下一帧激光数据重新计算
+        // 不继续执行后面的速度计算，避免使用旧数据
+        return;
+    }
     // 计算控制速度
     auto [linear_velocity, angular_velocity] = RechargeTool::computeDockingVelocity(dx - 0.3, dy, yaw_diff);
     RCLCPP_DEBUG(node_->get_logger(), "基础控制速度 - 线速度: %.4f, 角速度: %.4f",
                  linear_velocity, angular_velocity);
-    RCLCPP_INFO(node_->get_logger(),"dx=%.4f,dy=%.4f",dx,dy);
-    if(checkStationAlignment(dx,dy,yaw_diff))
-    {
-        //停止行车中，准备向前移动，再校准回充
-        sleep(100);
-        
-    }
-
+    RCLCPP_INFO(node_->get_logger(), "dx=%.4f,dy=%.4f", dx, dy);
     // 限制角速度
     if (angular_velocity > 0.0)
     {
@@ -537,11 +647,11 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
     }
 }
 
-bool LidarScanController::checkStationAlignment(double dx,double dy,double yaw_diff)
+bool LidarScanController::checkStationAlignment(double dx, double dy, double yaw_diff)
 {
-    bool isstop=false;
+    bool isstop = false;
     const double ALIGNMENT_DISTANCE_THRESHOLD = 0.80;            // 80cm
-    const double ALIGNMENT_ANGLE_THRESHOLD = 10.0 * M_PI / 180.0; // 5度
+    const double ALIGNMENT_ANGLE_THRESHOLD = 5.0 * M_PI / 180.0; // 5度
 
     if (dx <= ALIGNMENT_DISTANCE_THRESHOLD)
     {
@@ -549,18 +659,18 @@ bool LidarScanController::checkStationAlignment(double dx,double dy,double yaw_d
                     dx, yaw_diff * 180.0 / M_PI);
 
         // 判断是否对准（夹角小于10度）
-        if (fabs(yaw_diff) <= ALIGNMENT_ANGLE_THRESHOLD&&fabs(dy)<1.088)
+        if (fabs(yaw_diff) <= ALIGNMENT_ANGLE_THRESHOLD)
         {
             RCLCPP_INFO(node_->get_logger(), "✓ 已对准基站，继续行驶 - 夹角: %.2f°,dx=%.2f,dy=%.2f",
-                        yaw_diff * 180.0 / M_PI,dx,dy);
+                        yaw_diff * 180.0 / M_PI, dx, dy);
 
             RCLCPP_DEBUG(node_->get_logger(), "对准行驶 - 线速度: %.4f", ctrl_twist_.linear.x);
         }
         else
         {
             RCLCPP_WARN(node_->get_logger(), "✗ 未对准基站，停止行驶 - 夹角: %.2f° > 5°,dx=%.2f,dy=%.2f",
-                        yaw_diff * 180.0 / M_PI,dx,dy);
-            isstop=true;
+                        yaw_diff * 180.0 / M_PI, dx, dy);
+            isstop = true;
             // RCLCPP_INFO(node_->get_logger(), "自动调整方向 - 旋转角速度: %.4f", adjust_angular);
         }
     }

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

@@ -0,0 +1,2 @@
+第一种调到垂线范围（角度误差在5度）
+第二种种调到比垂线角度过大（角度误差比5度要大）