diff --git a/localization/pose2twist/README.md b/localization/pose2twist/README.md index 07d9c37b710fc..f1f7d6408fafb 100644 --- a/localization/pose2twist/README.md +++ b/localization/pose2twist/README.md @@ -5,7 +5,7 @@ This `pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging. The `twist.linear.x` is calculated as `sqrt(dx * dx + dy * dy + dz * dz) / dt`, and the values in the `y` and `z` fields are zero. -The `twist.angular` is calculated as `d_roll / dt`, `d_pitch / dt` and `d_yaw / dt` for each field. +The `twist.angular` is calculated as `relative_rotation_vector / dt` for each field. ## Inputs / Outputs diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index 1ffad737a8ad1..a3e537f99b085 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -69,7 +69,7 @@ geometry_msgs::msg::TwistStamped calc_twist( const auto pose_b_quaternion = get_quaternion(pose_b); geometry_msgs::msg::Vector3 diff_xyz; - const geometry_msgs::msg::Vector3 diff_rpy = + const geometry_msgs::msg::Vector3 relative_rotation_vector = compute_relative_rotation_vector(pose_a_quaternion, pose_b_quaternion); diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x; @@ -83,9 +83,9 @@ geometry_msgs::msg::TwistStamped calc_twist( dt; twist.twist.linear.y = 0; twist.twist.linear.z = 0; - twist.twist.angular.x = diff_rpy.x / dt; - twist.twist.angular.y = diff_rpy.y / dt; - twist.twist.angular.z = diff_rpy.z / dt; + twist.twist.angular.x = relative_rotation_vector.x / dt; + twist.twist.angular.y = relative_rotation_vector.y / dt; + twist.twist.angular.z = relative_rotation_vector.z / dt; return twist; }