正多边形巡航

基于相对位姿（OdomRelPose）发布的状态数据，实现“正多边形巡航”。节点订阅 rel_pose_json 话题。

节点在闭环中累计段内弧长并对齐目标航向，按 MOVE_SIDE × N → TURN_CORNER × N → HOME_TRANSLATE → HOME_TURN 的有限状态机执行，驱动底盘沿给定边长的 N 边形（默认正方形）运动，并最终回到起点与初始朝向。

内置节点 regular_polygon_cruise 订阅 rel_pose_json（相对位姿 JSON），并发布 cmd_vel（geometry_msgs/Twist）控制底盘。

启动运动底盘

以OriginBot智能小车为例，先启动小车底盘运动控制

shell

ros2 launch originbot_bringup originbot.launch.py

启动节点命令

shell

ros2 run pycodeapp regular_polygon_cruise

该节点默认绑定可以直接通过平台节点管理菜单启动。

节点参数说明

参数名	描述	类型	默认值	说明
rel_pose_topic	订阅主题	String	rel_pose_json	相对位姿 JSON（ std_msgs/String ），包含位移/航向/时间戳。
cmd_topic	速度指令主题	String	cmd_vel	发布 geometry_msgs/Twist 控制底盘。
side_length	每边长度	double	0.5	多边形每段累计弧长达到该值即转弯（m）。
num_sides	边数	int	4	正多边形边数（≥3；非法输入将回退为 4）。
pos_tol_m	回家位置容差	double	0.02	位置误差小于该值视为回到起点（m）。
yaw_tol_deg	角度容差	double	2.0	航向误差小于该值并稳定数周期视为到位（deg）。
v_forward	直行速度	double	0.1	直行阶段的线速度（m/s）。
w_turn	转弯最大角速度	double	0.4	角速度限幅（rad/s），用于转弯与纠偏。
k_yaw_move	直行偏航增益	double	1.2	直行段： yaw_err → w 的 P 增益。
k_turn	转弯/归正增益	double	2.0	转弯与归正阶段： yaw_err → w 的 P 增益。
k_home_v	回家线速度增益	double	0.8	回家阶段： distance → v 的比例增益。
k_home_w	回家角速度增益	double	2.0	回家阶段： yaw_err → w 的比例增益。
v_home_max	回家最大线速度	double	0.1	回家阶段线速度上限（m/s）。
w_home_max	回家最大角速度	double	0.8	回家阶段角速度上限（rad/s）。
control_rate_hz	控制频率	double	30.0	控制主循环周期（Hz）。
input_timeout_sec	输入超时	double	0.8	超过该秒数未收到输入即急停并回到等待状态。

参数使用方法

shell

# 自定义输入话题与多边形参数（边长/边数）及运动/增益/限幅 ros2 run pycodeapp regular_polygon_cruise --ros-args \ -p rel_pose_topic:=rel_pose_json \ -p cmd_topic:=cmd_vel \ -p side_length:=0.80 \ -p num_sides:=6 \ -p v_forward:=0.12 \ -p w_turn:=0.5 \ -p k_yaw_move:=1.4 \ -p k_turn:=2.2 \ -p pos_tol_m:=0.03 \ -p yaw_tol_deg:=2.0 \ -p v_home_max:=0.12 \ -p w_home_max:=0.9 # 提高控制频率并收紧输入超时（更快失联急停） ros2 run pycodeapp regular_polygon_cruise --ros-args \ -p control_rate_hz:=40.0 \ -p input_timeout_sec:=0.5

实时运动数据

进入该节点通信页面，可实时查看对底盘的控制数据。

点击发布主题“/cmd_vel”中的“订阅”按钮，类型选择“文字”，可查看当前线速度/角速度。