robofish/MOVE_AI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

改simple move

Browse Source
This commit is contained in:
Robofish 2026-03-11 08:36:21 +08:00
parent 76940c937b
commit cd23dce3d5
3 changed files with 28 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.github/copilot-instructions.md vendored Normal file
View File

@ -0,0 +1,6 @@
---
description: 当助手已完成当前请求的工作、准备结束回复或不再有明确下一步可执行内容时，必须先调用提问工具 askQuestions 获取进一步指示；不得直接结束对话。 / When the assistant has completed the current task and is about to end the reply or has no clear next action, it must call the askQuestions tool to obtain further instructions instead of ending the conversation.
applyTo: ‘当检测到回复将以收尾语句结束或无后续动作时 / When a reply is about to end with a closing statement or has no follow-up action’
---
当你完成工作时，请调用提问工具（askQuestions）来获取进一步的指示，而不是直接结束对话。
When you finish the task, call the askQuestions tool to request further instructions instead of ending the conversation.

5
nav.sh
View File

@ -4,11 +4,12 @@ commands=(
"ros2 launch rm_serial_driver rm_serial_driver.launch.py" "ros2 launch rm_serial_driver rm_serial_driver.launch.py"
"ros2 launch rm_decision decision.launch.py" "ros2 launch rm_decision decision.launch.py"
"ros2 launch rm_simpal_move simple_move.launch.py" "ros2 launch rm_simpal_move simple_move.launch.py"
"ros2 launch rm_nav_bringup bringup_real.launch.py \ "ros2 launch rm_nav_bringup bringup_sim.launch.py \
world:=C_ROOM \ world:=RMUL \
mode:=nav \ mode:=nav \
lio:=fastlio \ lio:=fastlio \
localization:=gicp \ localization:=gicp \
controller:=mppi \
lio_rviz:=False \ lio_rviz:=False \
nav_rviz:=True nav_rviz:=True
" "

31
src/rm_nav/rm_simple_move/src/simple_move.cpp
View File

@ -11,6 +11,12 @@ namespace rm_simpal_move
if (dt <= 0.0f) return 0.0f; if (dt <= 0.0f) return 0.0f;
integral += error * dt; integral += error * dt;
// 抗积分饱和：限制积分项幅值
float integral_limit = max_output / (ki > 1e-6f ? ki : 1.0f);
if (integral > integral_limit) integral = integral_limit;
if (integral < -integral_limit) integral = -integral_limit;
float derivative = (error - prev_error) / dt; float derivative = (error - prev_error) / dt;
prev_error = error; prev_error = error;
@ -65,7 +71,7 @@ namespace rm_simpal_move
nav_goal_sent_(false), nav_goal_sent_(false),
pid_linear_x_(0.8f, 0.0f, 0.1f, 3.0f), pid_linear_x_(0.8f, 0.0f, 0.1f, 3.0f),
pid_linear_y_(0.8f, 0.0f, 0.1f, 3.0f), pid_linear_y_(0.8f, 0.0f, 0.1f, 3.0f),
pid_angular_(1.5f, 0.0f, 0.2f, 2.0f) pid_angular_(0.8f, 0.0f, 0.1f, 2.0f)
{ {
// 声明参数 // 声明参数
this->declare_parameter("pid_linear_kp", 0.3); this->declare_parameter("pid_linear_kp", 0.3);
@ -225,10 +231,12 @@ namespace rm_simpal_move
dt = 0.1f; // 防止异常值 dt = 0.1f; // 防止异常值
} }
// 计算角度误差 // 从 base_link→goal_pose 的 TF 变换中直接提取角度误差
float angle_error = goal_pose_.angle - current_eulr_.yaw; // 这比分别用 map 坐标系下的绝对角度相减更准确
if (angle_error > M_PI) angle_error -= 2 * M_PI; AHRS_Eulr_t goal_eulr;
if (angle_error < -M_PI) angle_error += 2 * M_PI; AHRS_GetEulr(&goal_eulr, goal_in_base_link.transform.rotation);
float angle_error = goal_eulr.yaw;
// angle_error 已经是 base_link 到 goal_pose 的相对角度，天然在 [-π, π] 范围内
// PID 控制 // PID 控制
float vx = pid_linear_x_.compute(goal_x, dt); float vx = pid_linear_x_.compute(goal_x, dt);
@ -242,13 +250,12 @@ namespace rm_simpal_move
vx *= scale; vx *= scale;
vy *= scale; vy *= scale;
} }
// if (wz >goal_pose_.max_speed/2.0f) {
// wz = goal_pose_.max_speed; // 角速度限幅（独立于线速度限幅）
// } float max_angular_speed = goal_pose_.max_speed;
// if (wz < -goal_pose_.max_speed/2.0f) { if (wz > max_angular_speed) wz = max_angular_speed;
// wz = -goal_pose_.max_speed; if (wz < -max_angular_speed) wz = -max_angular_speed;
// }
wz = 0.0f;
// 发布标准 cmd_vel_move // 发布标准 cmd_vel_move
auto cmd_vel_msg = geometry_msgs::msg::Twist(); auto cmd_vel_msg = geometry_msgs::msg::Twist();
cmd_vel_msg.linear.x = vx; cmd_vel_msg.linear.x = vx;