Using util.angle_mod in all codes. #684

[videh25]

fix #684

As suggested in the issue, searched for instances for wrapping an angle between -pi to pi and modified the code to use util.angle_mod. Searched .pi throughout all the files to identify lines where angle_mod() is implemented again.

Summary of instances found:

File name	Function name/Line number	Action Taken
ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py	ang_diff	Modified
ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py	ang_diff	Modified
Control/move_to_pose/move_to_pose.py	Line 71	Modified
Localization/ensemble_kalman_filter/ensemble_kalman_filter.py	pi_2_pi	Modified
PathPlanning/ClosedLoopRRTStar/unicycle_model.py	pi_2_pi	Modified
PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py	pi_2_pi	Modified
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py	mod2pi	Deleted (Redundant)
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py	pi_2_pi	Modified
PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py	pi_2_pi	Modified
PathTracking/lqr_steer_control/lqr_steer_control.py	pi_2_pi	Modified
PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py	pi_2_pi	Modified
PathTracking/rear_wheel_feedback/rear_wheel_feedback.py	pi_2_pi	Modified
PathTracking/stanley_controller/stanley_controller.py	normalize_angle	Modified
SLAM/EKFSLAM/ekf_slam.py	pi_2_pi	Modified
SLAM/FastSLAM1/fast_slam1.py	pi_2_pi	Modified
SLAM/FastSLAM2/fast_slam2.py	pi_2_pi	Modified
SLAM/GraphBasedSLAM/graph_based_slam.py	pi_2_pi	Modified
SLAM/GraphBasedSLAM/graphslam/util.py	neg_pi_to_pi	No Action Taken (External Lib)

I hope this helps in review.

I am new to OSS contributions, please let me know if I am missing anything. Thanks for bearing with me! ^_^

CheckList

• Did you add an unittest for your new example or defect fix?
• Did you add documents for your new example?
• All CIs are green? (You can check it after submitting)

[AtsushiSakai]

Thank you for PR.
Please fix CI error first:

=================================== FAILURES ===================================
_____________________________________ test _____________________________________
[gw3] linux -- Python 3.11.6 /opt/hostedtoolcache/Python/3.11.6/x64/bin/python

    def test():
        branch_commit = find_branch_point("origin/master")
        files = diff_files(branch_commit)
        print(files)
        rc, errors = run_ruff(files, fix=True)
        if errors:
            print(errors)
        else:
            print("No lint errors found.")
>       assert rc == 0
E       assert 1 == 0

branch_commit = '0dc6ab42bab4d3e2fb06fc[25](https://github.com/AtsushiSakai/PythonRobotics/actions/runs/7062067522/job/19257122187?pr=946#step:6:26)d86b1abe41d4da2a'
errors     = 'Control/move_to_pose/move_to_pose.py:181:15: UP0[31](https://github.com/AtsushiSakai/PythonRobotics/actions/runs/7062067522/job/19257122187?pr=946#step:6:32) Use format specifiers instead of percent format\nControl/move_to_p...errors (9 fixed, 3 remaining).\nNo fixes available (3 hidden fixes can be enabled with the `--unsafe-fixes` option).\n'
files      = ['/home/runner/work/PythonRobotics/PythonRobotics/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_cont...'/home/runner/work/PythonRobotics/PythonRobotics/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py', ...]
rc         = 1

[videh25]

Fixed the CI errors as requested.

[AtsushiSakai]

Thank you for your PR. I have some recommendations. PTAL.

[AtsushiSakai]

LGTM. Thank you for long discussion with me!!