search

ivogeorg / robot_patrol

Checkpoints 5 & 6
0 stars 0 forks source link

Final robot yaw off by 180° from `theta` #1

Closed ivogeorg closed 1 week ago

ivogeorg commented 1 week ago

Often the world angle to which the robot rotates is off by 180 deg from theta of the action goal.

ivogeorg commented 1 week ago

It's actually more generally incorrect. Need to track and debug more extensively.

ivogeorg commented 1 week ago

After moving in the negative-x direction, the robot rotates to a theta with the opposite of the correct sign.

ivogeorg commented 1 week ago

Wrong:

[go_to_pose_action_node-1] [DEBUG] [1720493632.573046756] [gotopose_server]: (go_to) Final distance from goal: 0.081055
[go_to_pose_action_node-1] [DEBUG] [1720493632.573104262] [gotopose_server]: World angle to turn to = 0.000000
[go_to_pose_action_node-1] [DEBUG] [1720493632.573122805] [gotopose_server]: (rotate) Angle passed: 0.00
[go_to_pose_action_node-1] [DEBUG] [1720493632.573137661] [gotopose_server]: (rotate) World frame
[go_to_pose_action_node-1] [DEBUG] [1720493632.573151237] [gotopose_server]: (rotate) Current yaw: -90.01
[go_to_pose_action_node-1] [DEBUG] [1720493632.573167274] [gotopose_server]: (rotate) Goal angle: 90.01
[go_to_pose_action_node-1] [INFO] [1720493632.573258716] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720493633.073356077] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720493633.573329893] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720493634.073354601] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720493634.573346747] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720493635.073333062] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720493635.575121045] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720493636.073345574] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720493636.173321215] [gotopose_server]: (rotate) Angular difference from goal: 185.14
[go_to_pose_action_node-1] [INFO] [1720493636.173491731] [gotopose_server]: Goal succeeded
ivogeorg commented 1 week ago

Correct:

[go_to_pose_action_node-1] [DEBUG] [1720494951.743322254] [gotopose_server]: World angle to turn to = 0.000000
[go_to_pose_action_node-1] [DEBUG] [1720494951.743340296] [gotopose_server]: (rotate) Angle passed: 0.00
[go_to_pose_action_node-1] [DEBUG] [1720494951.743354078] [gotopose_server]: (rotate) World frame
[go_to_pose_action_node-1] [DEBUG] [1720494951.743366564] [gotopose_server]: (rotate) Current yaw: 167.99
[go_to_pose_action_node-1] [DEBUG] [1720494951.743382937] [gotopose_server]: (rotate) Goal angle: -167.99
[go_to_pose_action_node-1] [INFO] [1720494951.743448537] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494952.243579390] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494952.743551676] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494953.243595419] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494953.743558024] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494954.243561219] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494954.743567318] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494955.243570953] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494955.743569420] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494956.243596630] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494956.743595241] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494957.243620353] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494957.743547101] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720494957.843566564] [gotopose_server]: (rotate) Angular difference from goal: 1.73
[go_to_pose_action_node-1] [INFO] [1720494957.843767325] [gotopose_server]: Goal succeeded
ivogeorg commented 1 week ago

Correct:

[go_to_pose_action_node-1] [DEBUG] [1720495255.532833849] [gotopose_server]: World angle to turn to = -90.000000
[go_to_pose_action_node-1] [DEBUG] [1720495255.532848558] [gotopose_server]: (rotate) Angle passed: -90.00
[go_to_pose_action_node-1] [DEBUG] [1720495255.532860081] [gotopose_server]: (rotate) World frame
[go_to_pose_action_node-1] [DEBUG] [1720495255.532869317] [gotopose_server]: (rotate) Current yaw: -0.22
[go_to_pose_action_node-1] [DEBUG] [1720495255.532878946] [gotopose_server]: (rotate) Goal angle: -89.78
[go_to_pose_action_node-1] [DEBUG] [1720495255.532889001] [gotopose_server]: (rotate) Difference from goal: 0.22
[go_to_pose_action_node-1] [INFO] [1720495255.532951515] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720495256.033105308] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720495256.533084026] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720495257.033077353] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720495257.533068011] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720495258.033043771] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720495258.533075881] [gotopose_server]: Publish feedback
[go_to_pose_action_node-1] [INFO] [1720495258.833030068] [gotopose_server]: (rotate) Angular difference from goal: 2.37
[go_to_pose_action_node-1] [INFO] [1720495258.833236756] [gotopose_server]: Goal succeeded
ivogeorg commented 1 week ago

Velocity was being determined BEFORE finalization of the angle based on Frame::ROBOT and Frame::WORLD.