fake_vel_transform（云台系速度转换与底盘自旋）

1.fake_vel_transform（云台系速度转换与底盘自旋）

1.1.速度和旋转究竟参考谁

The navigation chain’s final vx、vy is expressed in the actual gimbal’s own gimbal_yaw frame; wz represents the target chassis angular velocity under the interface convention. The axes used to express velocity and the component executing movement are separate matters: translation expressed along gimbal axes still moves the chassis. The rotation command also does not directly control the gimbal yaw joint.

This comes from the design of the original Polaris package: first use a relatively stable virtual frame for navigation calculations, then convert the velocity into the actual gimbal frame;cmd_spin used to superimpose chassis spin.Polaris original package description、original package velocity conversion source code。This is the convention of this project; it cannot be assumed that all ROS2's cmd_vel are expressed in the gimbal frame.

vx、vy The unit is m/s, respectively along the x and y axes of the selected reference frame;wz The unit is rad/s, representing the rotational speed command about the z axis. The following explanation follows the convention of planar motion, with x forward, y to the left, and z upward; positive wz is counterclockwise when viewed from above. The actual vehicle axes, MCU sign polarity, and units still need to be verified.

The actual gimbal frame is an ordinary body frame that rotates with yaw, not a world coordinate frame. When the gimbal faces forward, its vx>0 points toward the front of the vehicle; when the gimbal turns 90° to the left, the same positive vx direction rotates to the left side of the vehicle. The chassis orientation remains unchanged, and this does not prevent using another set of axes to describe its motion.

gimbal_yaw_fake is a virtual frame used for computation: its position coincides with gimbal_yaw, the source code broadcasts gimbal_yaw → gimbal_yaw_fake, and its rotation takes the negative of the input yaw, with no translation. When the planar motion, input pose, and robot TF are correct and updated in a timely manner, its horizontal heading aligns with odom, while its position follows the robot. It is not map, nor is it the world origin of the gimbal or chassis; "the orientation aligns with odom" does not mean the complete coordinate frame coincides with odom. The source code only cancels yaw, not roll or pitch, so on a slope the fully horizontal conclusion cannot be directly applied.

1.1.1.一个直行例子：云台左转90°，车仍沿原方向走

Assume the chassis faces +x of odom and the gimbal originally also faces forward; now only the gimbal rotates 90° to the left. Navigation still requires moving along +x of odom with a speed of 0.3 m/s and a spin of 0:

The numbers change, but the intended motion does not. If the MCU interprets the received (0,-0.3) directly as chassis-frame components, it will understand the command as moving right relative to the robot. This example only explains coordinate conversion; no vehicle motion test was performed.

The source code uses the yaw in odometry.pose, currently expected to be the orientation of gimbal_yaw relative to odom; it includes both the overall orientation of the chassis and the effect of the mechanism relative to yaw; it is not a standalone mechanical joint angle. The linear velocity is calculated as:

vx_out =  cos(θ) × vx_in + sin(θ) × vy_in
vy_out = -sin(θ) × vx_in + cos(θ) × vy_in
θ      = 输入里程计位姿中的 yaw，单位 rad

When the MCU switches from the gimbal frame to the chassis frame, the mechanical relative angle between the two must be used additionally. If q represents the planar orientation of the gimbal frame relative to the chassis frame, with the fixed installation relationship already accounted for, the direction conversion is:

vx_chassis = cos(q) × vx_out - sin(q) × vy_out
vy_chassis = sin(q) × vx_out + cos(q) × vy_out
q          = 云台相对底盘的角度，单位 rad

θ is relative to odom, while q is relative to the chassis; they cannot be used interchangeably. MCU world-frame IMU orientation cannot be used as a joint angle. Determine q from mechanical relative yaw and the installation zero position. In the 90° example above, the chassis happens to align with odom, so the two angles are equal. Once the chassis also turns, they differ.

In this step, the host computer does not query the gimbal-to-chassis TF, nor does it handle rotational speed compensation between different origins; the MCU formula above only explains the axis swapping for planar directions. Whether origin offset needs to be handled must be verified against the actual installation and the lower-level controller's speed definitions.

1.1.2.wz：同一个底盘指令的两项相加

wz_final = wz_nav2 + wz_spin

Both units are the same, both are rad/s:

• wz_nav2: The rotational component proposed by navigation, such as adjusting heading; it is a control command, not a measured gimbal or chassis angular velocity.
• wz_spin: The chassis spin amount manually added, provided by startup parameters or cmd_spin messages.
• wz_final: The total target angular velocity sent to the chassis. No separate angular velocity is sent to each of the two mechanical components.

In planar motion, the gimbal and chassis differ only in horizontal orientation, with the same z-axis direction; the angular velocity component about z does not change due to horizontal yaw axis switching, so the source code only rotates vx、vy and directly adds wz. Here, the gimbal yaw is not differentiated, nor is the gimbal relative rotation speed used as the chassis rotation speed input. The overall chassis steering and the mechanical relative rotation of gimbal_yaw_joint must be understood separately and cannot be conflated into the same action based on topic reference frame names.

Currently, both the real robot and simulation FollowPath.enable_rotation are False. During normal path tracking, the navigation angular velocity output by this controller itself is 0. Therefore, before the spin setting has been changed, by default the real robot does not add spin, while the simulation adds 3.14 rad/s by default, about one revolution every 2 seconds. Nav2's behaviors such as recovery rotation have other command sources, and the velocity smoother may also retain transition processes, so it cannot be inferred that the entire navigation chain will always output only 0 angular velocity.

1.2.参数

1.2.1.ros__parameters (ROS node parameters)

1.2.1.1.Parameter table

This table is based on the current physical YAML; simulation differences are listed separately.local_plan_topic Not written in these two YAML files, but the source code declares and reads it, and currently uses its default value.

fake_vel_transform:
  ros__parameters:
    use_sim_time: False
    odom_topic: "odometry"
    robot_base_frame: "gimbal_yaw"
    fake_robot_base_frame: "gimbal_yaw_fake"
    input_cmd_vel_topic: "cmd_vel_nav2_result"
    output_cmd_vel_topic: "cmd_vel"
    cmd_spin_topic: "cmd_spin"
    init_spin_speed: 0.0

1.2.1.2.Parameter Details

1.2.1.2.1.odom_topic, robot_base_frame and fake_robot_base_frame (input odometry and real, virtual gimbal frames)

odom_topic Currently connected to sensor_scan_generation of the odometry; the message pose reference frame is expected to be odom and child_frame_id is gimbal_yaw. This node reads the yaw of this pose and publishes the inverse yaw transform from parent frame gimbal_yaw to child frame gimbal_yaw_fake at approximately 50 Hz, with zero translation.

Therefore, before changing the topic or the coordinate frame name, you must simultaneously check whose pose the input actually is. Renaming will not automatically turn a chassis pose into a gimbal pose, nor will it turn a world IMU attitude into joint angles. The source code does not query or validate these relationships; documented issues upstream, such as the internal IMU-to-lidar transform and mechanical TF failure returning an identity transform, may also cause incorrect angles to be used here.

MID360 continues to be mounted on gimbal_yawand rotates with the actual yaw but not with the actual pitch; the actual yaw and pitch are only in gimbal_yaw_joint and gimbal_pitch_joint respectively, and both odom joints are fixed. The virtual frame does not add a second actual yaw, and the serial JointState is still filled according to the mechanical relative angle as {relative_pitch, relative_yaw, 0, 0}.

1.2.1.2.2.input_cmd_vel_topic, output_cmd_vel_topic cooperate with the MCU (speed input and output topics)

The current tracking controller transforms the path into the body frame of the local costmap; this frame is configured as gimbal_yaw_fake to calculate the x and y velocities accordingly. The tracking output passes through cmd_vel_controller → velocity_smoother → cmd_vel_nav2_result and then enters this node; navigation behaviors can also output to cmd_vel_nav2_result.

The transformed published cmd_vel is a geometry_msgs/msg/Twist message without an explicit coordinate frame or timestamp. Although the output variable name contains chassis, the code only rotates planar components using input yaw. It does not further transform velocity into base_footprint or chassis. Determine direction from configuration, calculations and upstream and downstream conventions. Neither a variable name nor eventual chassis use establishes that velocity is already in the chassis frame.

serial_comm Directly write linear.x、linear.y、angular.z into the protocol field and send it periodically, without using JointState to change direction again. The MCU needs to verify four things: whether the linear velocity is received in the gimbal frame; the zero position and sign of the mechanical relative yaw; the units of m/s and rad/s; whether the angular velocity already includes spin and whether it will be added repeatedly. Currently, the MCU execution code or physical response has not been verified, so the host computer convention cannot be written as already correctly implemented on the lower computer.

This reference frame conclusion applies only to the above navigation chain. If the handle, etc., bypasses this node and publishes directly cmd_vel, it does not automatically obtain the same conversion; their velocity reference frame and the usage of topics with the same name need to be verified separately, and this block does not expand on the modification.

1.2.1.2.3.init_spin_speed and cmd_spin_topic: what is changed is the additional amount

Current physical value init_spin_speed=0.0; simulation is 3.14. This section originally uniformly stated “default 3.14 rad/s”; it is now distinguished according to the two configurations. The earlier “Data Integration” section still retains the 3.14 example, which does not represent the current physical default value; this round does not rewrite that old section. The Polar Bear official physical YAML currently uses 3.14, which is the original package setting; the specific value for the current project should be based on this table.Polar Bear official configuration

The startup initial value is only used to assign a value to spin_speed_; after receiving cmd_spin, it is directly replaced with the message value and retained thereafter. The source code has no automatic restoration of the initial value or message expiration rule. The startup parameter is used again only on restart. The following topic usage example is retained for illustration only and was not executed in this round:

# 设置附加自旋速度为 +1 rad/s，约每秒 57.3°
ros2 topic pub --once /cmd_spin example_interfaces/msg/Float32 "{data: 1.0}"

# 改为反方向自旋
ros2 topic pub --once /cmd_spin example_interfaces/msg/Float32 "{data: -1.0}"

# 关闭附加自旋
ros2 topic pub --once /cmd_spin example_interfaces/msg/Float32 "{data: 0.0}"

The above is an example with no namespace. Relative topic names are affected by the startup namespace; a common simulation uses /red_standard_robot1/cmd_spin; the actual namespace depends on the one used at startup, and a physical system is not necessarily using topic names in the root directory.

Set the additional amount to 0, only disable the spin additional term; even if the input velocity is all zeros, the spin additional term will not be automatically cleared. When the input is all zeros and spin_speed_≠0 , the conversion will still output a nonzero angular velocity. Receiving cmd_spin only changes the variable and does not immediately publish a new cmd_vel; the new value is only reflected after entering the velocity processing callback. It itself is not an independent rotation controller that continuously outputs velocity.

The source code adds the values directly, without limiting or smoothing the final angular velocity. The upstream smoother processes the velocities before addition, so its limits cannot be used to conclude that the total after addition is subject to the same limits. Whether it is suitable for a real vehicle must be checked against the chassis capability and lower-level controller limits; in this round, the rotational speed will not be adjusted.

1.2.1.2.4.local_plan_topic, use_sim_time and source code doubts

This node approximately pairs Odometry and Path; it is not a timestamped velocity. The input velocity has no timestamp, and only the most recent value is cached. Receiving a local path refreshes the "controller active" time. When there is a path within the last 0.5 seconds, a nonzero velocity waits for the pairing callback to process it; otherwise, it is directly converted using the currently saved yaw. Approximate pairing cannot prove that the path, pose, and velocity correspond to the same instant in time.

The following are gaps that can be confirmed from the source code; the actual impact has not yet been verified by running it:

• First-frame direction: current_robot_base_angle_ is not explicitly initialized, and there is no valid-yaw guard before publishing TF or converting velocity. Results before the first valid odometry message cannot be treated as reliable.
• Stale velocity reuse: The pairing callback does not clear cached velocity after using it. Zero velocity takes the direct-publication branch but does not clear an earlier cached nonzero command. A later pairing callback can therefore publish the old command again.
• Timeout semantics: The hardcoded 0.5-second check only checks recent local-path reception, not velocity-command freshness. It is not an automatic stopping mechanism. The 20 ms timer only publishes TF, without periodically publishing stop commands.
• Mixed clocks: Activity checks use the default rclcpp::Clock(), which is system time in Humble. TF timestamps use the node clock and can be affected by use_sim_time , while the timer uses wall time. True does not make these steps consistently use simulation time.

This does not expand on the previously deferred navigation walk-stop or map→odom timing troubleshooting, and only records the code prerequisites of this node.

1.2.1.2.5.Startup source and current verification scope

Complete reality and simulation launches default to nav2_params.yaml in their respective directories; an alternative can be specified using params_file . The navigation launch loads the processed parameter file into the node or component and overrides the existing use_sim_time setting. Both modes name the node fake_vel_transform to match the YAML.

The package’s standalone fake_vel_transform_launch.py only passes use_sim_time, defaulting to True, without reading the complete YAML. Other settings use C++ defaults, such as reference frame gimbal_link/gimbal_link_fake, odometry odom, and spinning component 0; input and output velocity topics still default to empty strings. This standalone entry point is not a fully configured navigation launch.

The 8 custom parameters are read only at construction time, with no dynamic parameter update callback; writing a parameter with the same name at runtime will not update members, rebuild the topic, or change the initial spin. ROS standard use_sim_time is handled by the node clock; changing the spin at runtime should be done via cmd_spin; do not confuse the two.

In this round, only the environment, installation path, and files were queried in ysu_ros_humble, confirming that the standalone launch, navigation launch, and both YAML files are consistent with the current source code files, and verifying the default clock type for Humble; no nodes were launched, the above messages were not sent, and no motion tests were performed.

• Source code can confirm: the reference frame conventions for navigation input and output, 2D velocity axis conversion, direct addition of angular velocity, raw serial port value passing, default differences between physical and simulation, parameter reading method, and the above code gaps.
• Simulation has observed: On 2026-10-08, only the stationary state of an independent Point-LIO and a round trip of about 20 cm were checked; this node, independent gimbal yaw, MCU velocity conversion, additional spin, or stopping behavior were not verified. The 90° example and addition table in this section are both calculation examples.
• Must be verified with physical hardware or lower-level controller source code: axis directions and units, the mechanical relative yaw zero position and direction, whether the input pose is correct, whether the MCU converts gimbal-frame linear velocity to the chassis execution direction, the execution target and limits of the final angular velocity, and whether spin is repeatedly superimposed. The previously noted issues regarding gravity direction and point-time units also remain to be verified.

Source code location: src/pb2025_sentry_nav/fake_vel_transform/src/fake_vel_transform.cpp, corresponding header files, and README; upstream: sensor_scan_generation、pb_omni_pid_pursuit_controller, and navigation launch; serial port: src/serial_comm/src/serial_comm.cpp. This round only modifies the logging parameter block; project source code, configuration, and parameter values are all preserved.