Skip to content

Commit

Permalink
remove rpy description
Browse files Browse the repository at this point in the history
Signed-off-by: Kento Yabuuchi <[email protected]>
  • Loading branch information
KYabuuchi committed Jun 7, 2024
1 parent 4770f94 commit af089e0
Show file tree
Hide file tree
Showing 2 changed files with 5 additions and 5 deletions.
2 changes: 1 addition & 1 deletion localization/pose2twist/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
8 changes: 4 additions & 4 deletions localization/pose2twist/src/pose2twist_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
}
Expand Down

0 comments on commit af089e0

Please sign in to comment.