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

simple move add config

Browse Source
This commit is contained in:
Robofish 2026-03-12 00:41:14 +08:00
parent cd23dce3d5
commit ac92e49cee
8 changed files with 128 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,3 @@
{
"cmake.sourceDirectory": "/home/robofish/MOVE_AI/src/rm_decision"
}

5
nav.sh
View File

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

4
src/rm_nav/rm_simple_move/CMakeLists.txt
View File

@ -36,8 +36,8 @@ install(TARGETS rm_simpal_move
RUNTIME DESTINATION bin
)
# install launch files
install(DIRECTORY launch
# install launch and config files
install(DIRECTORY launch config
DESTINATION share/${PROJECT_NAME}/
)

10
src/rm_nav/rm_simple_move/README.md
View File

@ -124,11 +124,11 @@ python3 move_test_new.py
**PID 模式：**
```bash
ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
control_mode: 1,
x: -2.0,
y: -1.0,
angle: 180.0,
max_speed: 1.0,
control_mode: 0,
x: 1.0,
y: -5.0,
angle: 0.0,
max_speed: 2.0,
tolerance: 0.1
}" --once
```

35
src/rm_nav/rm_simple_move/config/simple_move.yaml Normal file
View File

@ -0,0 +1,35 @@
rm_simple_move:
ros__parameters:
# 通用参数
use_sim_time: false
control_frequency: 10.0 # 控制循环频率 (Hz)
# 话题配置
nav_goal_topic: "/nav_goal"
cmd_vel_input_topic: "/cmd_vel"
cmd_vel_output_topic: "/cmd_vel_move"
nav_status_topic: "/nav_status"
# PID 线速度 X
pid_linear_x:
kp: 0.8
ki: 0.0
kd: 0.1
max_output: 3.0
# PID 线速度 Y
pid_linear_y:
kp: 0.8
ki: 0.0
kd: 0.1
max_output: 3.0
# PID 角速度
pid_angular:
kp: 0.0005
ki: 0.0
kd: 0.0001
max_output: 0.05
# 最大角速度限幅 (rad/s)
max_angular_speed: 0.05

2
src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp
View File

@ -102,6 +102,8 @@ private:
PIDController pid_linear_y_;
PIDController pid_angular_;
double max_angular_speed_; // 角速度独立限幅 (rad/s)
rclcpp::Time last_time_;
};

13
src/rm_nav/rm_simple_move/launch/simple_move.launch.py
View File

@ -6,6 +6,9 @@ from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
def generate_launch_description():
pkg_dir = get_package_share_directory('rm_simpal_move')
config_file = os.path.join(pkg_dir, 'config', 'simple_move.yaml')
return LaunchDescription([
ComposableNodeContainer(
name='simple_move_container',
@ -17,15 +20,7 @@ def generate_launch_description():
package='rm_simpal_move',
plugin='rm_simpal_move::RMSimpleMove',
name='rm_simple_move',
parameters=[
{'use_sim_time': False},
{'pid_linear_kp': 0.8},
{'pid_linear_ki': 0.0},
{'pid_linear_kd': 0.1},
{'pid_angular_kp': 1.5},
{'pid_angular_ki': 0.0},
{'pid_angular_kd': 0.2}
]
parameters=[config_file]
),
],
output='screen',

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

@ -64,63 +64,106 @@ namespace rm_simpal_move
: Node("rm_simple_move", options),
tf_buffer_(this->get_clock()),
tf_listener_(tf_buffer_),
control_mode_(0), // 默认 PID 模式
control_mode_(0),
running_(true),
goal_reached_(false),
has_goal_(false),
nav_goal_sent_(false),
pid_linear_x_(0.8f, 0.0f, 0.1f, 3.0f),
pid_linear_y_(0.8f, 0.0f, 0.1f, 3.0f),
pid_angular_(0.8f, 0.0f, 0.1f, 2.0f)
nav_goal_sent_(false)
{
// 声明参数
this->declare_parameter("pid_linear_kp", 0.3);
this->declare_parameter("pid_linear_ki", 0.0);
this->declare_parameter("pid_linear_kd", 0.1);
this->declare_parameter("pid_angular_kp", 1.5);
this->declare_parameter("pid_angular_ki", 0.0);
this->declare_parameter("pid_angular_kd", 0.2);
// ========== 声明所有参数（默认值与 yaml 一致） ==========
// 通用
this->declare_parameter("control_frequency", 10.0);
// 读取参数
pid_linear_x_.kp = pid_linear_y_.kp = this->get_parameter("pid_linear_kp").as_double();
pid_linear_x_.ki = pid_linear_y_.ki = this->get_parameter("pid_linear_ki").as_double();
pid_linear_x_.kd = pid_linear_y_.kd = this->get_parameter("pid_linear_kd").as_double();
pid_angular_.kp = this->get_parameter("pid_angular_kp").as_double();
pid_angular_.ki = this->get_parameter("pid_angular_ki").as_double();
pid_angular_.kd = this->get_parameter("pid_angular_kd").as_double();
// 话题
this->declare_parameter("nav_goal_topic", std::string("/nav_goal"));
this->declare_parameter("cmd_vel_input_topic", std::string("/cmd_vel"));
this->declare_parameter("cmd_vel_output_topic", std::string("/cmd_vel_move"));
this->declare_parameter("nav_status_topic", std::string("/nav_status"));
// PID 线速度 X
this->declare_parameter("pid_linear_x.kp", 0.8);
this->declare_parameter("pid_linear_x.ki", 0.0);
this->declare_parameter("pid_linear_x.kd", 0.1);
this->declare_parameter("pid_linear_x.max_output", 3.0);
// PID 线速度 Y
this->declare_parameter("pid_linear_y.kp", 0.8);
this->declare_parameter("pid_linear_y.ki", 0.0);
this->declare_parameter("pid_linear_y.kd", 0.1);
this->declare_parameter("pid_linear_y.max_output", 3.0);
// PID 角速度
this->declare_parameter("pid_angular.kp", 0.0005);
this->declare_parameter("pid_angular.ki", 0.0);
this->declare_parameter("pid_angular.kd", 0.0001);
this->declare_parameter("pid_angular.max_output", 0.05);
// 最大角速度
this->declare_parameter("max_angular_speed", 0.05);
// ========== 读取参数 ==========
double control_freq = this->get_parameter("control_frequency").as_double();
std::string nav_goal_topic = this->get_parameter("nav_goal_topic").as_string();
std::string cmd_vel_input_topic = this->get_parameter("cmd_vel_input_topic").as_string();
std::string cmd_vel_output_topic = this->get_parameter("cmd_vel_output_topic").as_string();
std::string nav_status_topic = this->get_parameter("nav_status_topic").as_string();
pid_linear_x_ = PIDController(
this->get_parameter("pid_linear_x.kp").as_double(),
this->get_parameter("pid_linear_x.ki").as_double(),
this->get_parameter("pid_linear_x.kd").as_double(),
this->get_parameter("pid_linear_x.max_output").as_double()
);
pid_linear_y_ = PIDController(
this->get_parameter("pid_linear_y.kp").as_double(),
this->get_parameter("pid_linear_y.ki").as_double(),
this->get_parameter("pid_linear_y.kd").as_double(),
this->get_parameter("pid_linear_y.max_output").as_double()
);
pid_angular_ = PIDController(
this->get_parameter("pid_angular.kp").as_double(),
this->get_parameter("pid_angular.ki").as_double(),
this->get_parameter("pid_angular.kd").as_double(),
this->get_parameter("pid_angular.max_output").as_double()
);
max_angular_speed_ = this->get_parameter("max_angular_speed").as_double();
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
// 定时器 10Hz
// 定时器
int period_ms = static_cast<int>(1000.0 / control_freq);
timer_ = this->create_wall_timer(
std::chrono::milliseconds(100),
std::chrono::milliseconds(period_ms),
std::bind(&RMSimpleMove::timer_callback, this)
);
// 订阅者
nav_goal_sub_ = this->create_subscription<rm_msgs::msg::NavGoal>(
"/nav_goal", 10,
nav_goal_topic, 10,
std::bind(&RMSimpleMove::nav_goal_callback, this, std::placeholders::_1)
);
cmd_vel_nav_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", 10,
cmd_vel_input_topic, 10,
std::bind(&RMSimpleMove::cmd_vel_nav_callback, this, std::placeholders::_1)
);
// 发布者
cmd_vel_move_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel_move", 10);
nav_status_pub_ = this->create_publisher<rm_msgs::msg::NavStatus>("/nav_status", 10);
cmd_vel_move_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(cmd_vel_output_topic, 10);
nav_status_pub_ = this->create_publisher<rm_msgs::msg::NavStatus>(nav_status_topic, 10);
// Action Client
nav_action_client_ = rclcpp_action::create_client<NavigateToPose>(this, "/navigate_to_pose");
RCLCPP_INFO(this->get_logger(), "话题配置:");
RCLCPP_INFO(this->get_logger(), " 输入: /nav_goal");
RCLCPP_INFO(this->get_logger(), " 输出: /cmd_vel_move (统一输出)");
RCLCPP_INFO(this->get_logger(), " PID模式: 直接计算发布到 /cmd_vel_move");
RCLCPP_INFO(this->get_logger(), " NAV模式: Action /navigate_to_pose → 导航 → /cmd_vel → 转发到 /cmd_vel_move");
RCLCPP_INFO(this->get_logger(), " 状态: /nav_status");
RCLCPP_INFO(this->get_logger(), " 输入: %s", nav_goal_topic.c_str());
RCLCPP_INFO(this->get_logger(), " 输出: %s (统一输出)", cmd_vel_output_topic.c_str());
RCLCPP_INFO(this->get_logger(), " PID模式: 直接计算发布到 %s", cmd_vel_output_topic.c_str());
RCLCPP_INFO(this->get_logger(), " NAV模式: Action /navigate_to_pose → 导航 → %s → 转发到 %s",
cmd_vel_input_topic.c_str(), cmd_vel_output_topic.c_str());
RCLCPP_INFO(this->get_logger(), " 状态: %s", nav_status_topic.c_str());
RCLCPP_INFO(this->get_logger(), " 控制频率: %.1f Hz", control_freq);
last_time_ = this->get_clock()->now();
@ -252,9 +295,8 @@ namespace rm_simpal_move
}
// 角速度限幅（独立于线速度限幅）
float max_angular_speed = goal_pose_.max_speed;
if (wz > max_angular_speed) wz = max_angular_speed;
if (wz < -max_angular_speed) wz = -max_angular_speed;
if (wz > max_angular_speed_) wz = max_angular_speed_;
if (wz < -max_angular_speed_) wz = -max_angular_speed_;
// 发布标准 cmd_vel_move
auto cmd_vel_msg = geometry_msgs::msg::Twist();