Fix path planning tests (#499)

bitbots_navigation/bitbots_path_planning/test/__snapshots__/test_controller.ambr

@@ -1,19 +1,18 @@
 # serializer version: 1
 # name: test_default_setup
   dict({
-    'config_base_footprint_frame': 'base_footprint_frame',
-    'config_carrot_distance': 20,
-    'config_max_rotation_vel': 0.4,
-    'config_max_vel_x': 0.25,
-    'config_max_vel_y': 0.08,
-    'config_min_vel_x': -0.2,
-    'config_orient_to_goal_distance': 1.0,
-    'config_rotation_i_factor': 0.05,
-    'config_rotation_slow_down_factor': 0.3,
-    'config_smoothing_tau': 0.2,
-    'config_translation_slow_down_factor': 0.5,
+    'carrot_distance': 20,
+    'max_rotation_vel': 0.4,
+    'max_vel_x': 0.15,
+    'max_vel_y': 0.1,
+    'min_vel_x': -0.1,
+    'orient_to_goal_distance': 1.0,
+    'rotation_i_factor': 0.0,
+    'rotation_slow_down_factor': 0.3,
+    'smoothing_tau': 0.5,
+    'translation_slow_down_factor': 0.5,
   })
 # ---
 # name: test_step_cmd_vel_smoothing
-  'geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1375935868207597, y=0.08, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.2320828999230522))'
+  'geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.15, y=0.09999999999999996, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.20666164469042259))'
 # ---

bitbots_navigation/bitbots_path_planning/test/test_controller.py

@@ -1,117 +1,116 @@
-from types import SimpleNamespace
 from unittest.mock import Mock
 
 import pytest
 import tf2_ros as tf2
 from bitbots_path_planning.controller import Controller
+from bitbots_path_planning.path_planning_parameters import bitbots_path_planning as gen_params
 from geometry_msgs.msg import Twist
 from nav_msgs.msg import Path
 from rclpy.node import Node
 from rclpy.time import Time
 from tf2_geometry_msgs import PoseStamped
 
 
-def test_default_setup(snapshot, node, tf2_buffer):
+def test_default_setup(snapshot, node, tf2_buffer, config):
     controller = setup_controller(node, tf2_buffer)
     parameter_keys = [
-        "config_base_footprint_frame",
-        "config_carrot_distance",
-        "config_max_rotation_vel",
-        "config_max_vel_x",
-        "config_max_vel_y",
-        "config_min_vel_x",
-        "config_orient_to_goal_distance",
-        "config_rotation_slow_down_factor",
-        "config_rotation_i_factor",
-        "config_smoothing_tau",
-        "config_translation_slow_down_factor",
+        "carrot_distance",
+        "max_rotation_vel",
+        "max_vel_x",
+        "max_vel_y",
+        "min_vel_x",
+        "orient_to_goal_distance",
+        "rotation_slow_down_factor",
+        "rotation_i_factor",
+        "smoothing_tau",
+        "translation_slow_down_factor",
     ]
-    parameters = {p: getattr(controller, p) for p in parameter_keys}
+    parameters = {p: getattr(config.controller, p) for p in parameter_keys}
 
     assert controller.angular_error_accumulator == 0
     assert controller.last_cmd_vel == Twist()
     assert parameters == snapshot
 
 
-def test_step_limits_forward_x_velocity(node, tf2_buffer, pose_opponent_goal):
+def test_step_limits_forward_x_velocity(node, tf2_buffer, config, pose_opponent_goal):
     controller = setup_controller(node, tf2_buffer)
 
     controller.step(path_to(pose_opponent_goal))
 
-    assert controller.last_cmd_vel.linear.x == controller.config_max_vel_x
+    assert controller.last_cmd_vel.linear.x == config.controller.max_vel_x
     assert controller.last_cmd_vel.linear.y == pytest.approx(0)
     assert controller.last_cmd_vel.linear.z == 0
 
 
-def test_step_limits_backward_x_velocity(node, tf2_buffer, pose_own_goal):
+def test_step_limits_backward_x_velocity(node, tf2_buffer, config, pose_own_goal):
     controller = setup_controller(node, tf2_buffer)
 
     controller.step(path_to(pose_own_goal))
 
-    assert controller.last_cmd_vel.linear.x == controller.config_min_vel_x
+    assert controller.last_cmd_vel.linear.x == config.controller.min_vel_x
     assert controller.last_cmd_vel.linear.y == pytest.approx(0)
     assert controller.last_cmd_vel.linear.z == 0
 
 
-def test_step_limits_forward_y_velocity(node, tf2_buffer, pose_left_line):
+def test_step_limits_forward_y_velocity(node, tf2_buffer, config, pose_left_line):
     controller = setup_controller(node, tf2_buffer)
 
     controller.step(path_to(pose_left_line))
 
     assert controller.last_cmd_vel.linear.x == pytest.approx(0)
-    assert controller.last_cmd_vel.linear.y == controller.config_max_vel_y
+    assert controller.last_cmd_vel.linear.y == config.controller.max_vel_y
     assert controller.last_cmd_vel.linear.z == 0
 
 
-def test_step_limits_backward_y_velocity(node, tf2_buffer, pose_right_line):
+def test_step_limits_backward_y_velocity(node, tf2_buffer, config, pose_right_line):
     controller = setup_controller(node, tf2_buffer)
 
     controller.step(path_to(pose_right_line))
 
     assert controller.last_cmd_vel.linear.x == pytest.approx(0)
-    assert controller.last_cmd_vel.linear.y == -controller.config_max_vel_y
+    assert controller.last_cmd_vel.linear.y == -config.controller.max_vel_y
     assert controller.last_cmd_vel.linear.z == 0
 
 
-def test_step_limits_forward_xy_velocities(node, tf2_buffer, pose_opponent_corner):
+def test_step_limits_forward_xy_velocities(node, tf2_buffer, config, pose_opponent_corner):
     controller = setup_controller(node, tf2_buffer)
 
     controller.step(path_to(pose_opponent_corner))
 
-    assert controller.last_cmd_vel.linear.x == pytest.approx(0.12)
-    assert controller.last_cmd_vel.linear.y == controller.config_max_vel_y
+    assert controller.last_cmd_vel.linear.x == pytest.approx(config.controller.max_vel_x)
+    assert controller.last_cmd_vel.linear.y == pytest.approx(config.controller.max_vel_y)
     assert controller.last_cmd_vel.linear.z == 0
 
 
-def test_step_limits_backward_xy_velocities(node, tf2_buffer, pose_own_corner):
+def test_step_limits_backward_xy_velocities(node, tf2_buffer, config, pose_own_corner):
     controller = setup_controller(node, tf2_buffer)
 
     controller.step(path_to(pose_own_corner))
 
-    assert controller.last_cmd_vel.linear.x == pytest.approx(-0.12)
-    assert controller.last_cmd_vel.linear.y == -controller.config_max_vel_y
+    assert controller.last_cmd_vel.linear.x == pytest.approx(config.controller.min_vel_x)
+    assert controller.last_cmd_vel.linear.y == pytest.approx(-0.066666666)
     assert controller.last_cmd_vel.linear.z == 0
 
 
-def test_step_limits_rotation(node, tf2_buffer, pose_left_line, pose_right_line):
+def test_step_limits_rotation(node, tf2_buffer, config, pose_left_line, pose_right_line):
     controller = setup_controller(node, tf2_buffer)
 
     controller.step(path_to(pose_left_line))
-    assert controller.last_cmd_vel.angular.z == controller.config_max_rotation_vel
+    assert controller.last_cmd_vel.angular.z == config.controller.max_rotation_vel
 
     controller.last_update_time = None
     controller.step(path_to(pose_right_line))
-    assert controller.last_cmd_vel.angular.z == -controller.config_max_rotation_vel
+    assert controller.last_cmd_vel.angular.z == -config.controller.max_rotation_vel
 
 
-def test_step_cmd_vel_smoothing(snapshot, node, tf2_buffer, pose_opponent_corner):
+def test_step_cmd_vel_smoothing(snapshot, node, tf2_buffer, config, pose_opponent_corner):
     controller = setup_controller(node, tf2_buffer)
-    controller.config_smoothing_tau = 0.5
+    controller.node.config.controller.smoothing_tau = 0.5
     controller.last_update_time = Time(seconds=0)
 
-    controller.last_cmd_vel.linear.x = controller.config_max_vel_x
-    controller.last_cmd_vel.linear.y = controller.config_max_vel_y
-    controller.last_cmd_vel.angular.z = controller.config_max_rotation_vel
+    controller.last_cmd_vel.linear.x = config.controller.max_vel_x
+    controller.last_cmd_vel.linear.y = config.controller.max_vel_y
+    controller.last_cmd_vel.angular.z = config.controller.max_rotation_vel
 
     controller.step(path_to(pose_opponent_corner))
 
@@ -209,10 +208,15 @@ def pose_left_line() -> PoseStamped:
 
 
 @pytest.fixture
-def node() -> Node:
+def config() -> gen_params.Params:
+    return gen_params.Params()
+
+
+@pytest.fixture
+def node(config) -> Node:
     node = Mock(Node)
-    node.get_parameter = lambda key: SimpleNamespace(value=key)
-    node.declare_parameter = lambda _, default_value: SimpleNamespace(value=default_value)
+
+    node.config = config
 
     node.get_logger.return_value.debug = print
     node.get_clock.return_value.now.return_value = Time(seconds=1)