From 1579a24906e5ff4cc3fb81bb877665cef522e27c Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Wed, 15 Jan 2025 13:08:33 +0100 Subject: [PATCH 1/3] rename head modes more clearly --- .../bitbots_blackboard/capsules/misc_capsule.py | 8 ++++---- .../behavior_dsd/actions/head_modes.py | 8 ++++---- .../bitbots_teleop/scripts/teleop_keyboard.py | 16 ++++++++-------- .../bitbots_head_mover/src/move_head.cpp | 8 ++++---- bitbots_msgs/msg/HeadMode.msg | 8 ++++---- 5 files changed, 24 insertions(+), 24 deletions(-) diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py index 90107d664..51f7eb197 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py +++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py @@ -8,12 +8,12 @@ from bitbots_msgs.msg import Audio, HeadMode, RobotControlState THeadMode: TypeAlias = Literal[ # type: ignore[valid-type] - HeadMode.BALL_MODE, - HeadMode.FIELD_FEATURES, + HeadMode.SEARCH_BALL, + HeadMode.SEARCH_FIELD_FEATURES, HeadMode.LOOK_FORWARD, HeadMode.DONT_MOVE, - HeadMode.BALL_MODE_PENALTY, - HeadMode.LOOK_FRONT, + HeadMode.SEARCH_BALL_PENALTY, + HeadMode.SEARCH_FRONT, ] diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/head_modes.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/head_modes.py index a2948d98b..ad5eb3075 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/head_modes.py +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/head_modes.py @@ -45,7 +45,7 @@ class SearchBall(AbstractHeadModeElement): """Look for ball""" def perform(self): - self.blackboard.misc.set_head_duty(HeadMode.BALL_MODE) + self.blackboard.misc.set_head_duty(HeadMode.SEARCH_BALL) return self.pop() @@ -53,7 +53,7 @@ class LookAtFieldFeatures(AbstractHeadModeElement): """Look generally for all features on the field (ball, goals, corners, center point)""" def perform(self): - self.blackboard.misc.set_head_duty(HeadMode.FIELD_FEATURES) + self.blackboard.misc.set_head_duty(HeadMode.SEARCH_FIELD_FEATURES) return self.pop() @@ -77,7 +77,7 @@ class LookAtBallPenalty(AbstractHeadModeElement): """Ball Mode adapted for Penalty Kick""" def perform(self): - self.blackboard.misc.set_head_duty(HeadMode.BALL_MODE_PENALTY) + self.blackboard.misc.set_head_duty(HeadMode.SEARCH_BALL_PENALTY) return self.pop() @@ -85,5 +85,5 @@ class LookAtFront(AbstractHeadModeElement): """Search in front of the robot""" def perform(self): - self.blackboard.misc.set_head_duty(HeadMode.LOOK_FRONT) + self.blackboard.misc.set_head_duty(HeadMode.SEARCH_FRONT) return self.pop() diff --git a/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py b/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py index 341518df0..f88fadad4 100755 --- a/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py +++ b/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py @@ -222,12 +222,12 @@ def loop(self): self.head_pub.publish(self.head_msg) elif key == "0": # Search for Ball and track it if found - self.head_mode_msg.head_mode = HeadMode.BALL_MODE - assert int(key) == HeadMode.BALL_MODE + self.head_mode_msg.head_mode = HeadMode.SEARCH_BALL + assert int(key) == HeadMode.SEARCH_BALL elif key == "1": # Look generally for all features on the field (ball, goals, corners, center point) - self.head_mode_msg.head_mode = HeadMode.FIELD_FEATURES - assert int(key) == HeadMode.FIELD_FEATURES + self.head_mode_msg.head_mode = HeadMode.SEARCH_FIELD_FEATURES + assert int(key) == HeadMode.SEARCH_FIELD_FEATURES elif key == "2": # Simply look directly forward self.head_mode_msg.head_mode = HeadMode.LOOK_FORWARD @@ -238,12 +238,12 @@ def loop(self): assert int(key) == HeadMode.DONT_MOVE elif key == "4": # Ball Mode adapted for Penalty Kick - self.head_mode_msg.head_mode = HeadMode.BALL_MODE_PENALTY - assert int(key) == HeadMode.BALL_MODE_PENALTY + self.head_mode_msg.head_mode = HeadMode.SEARCH_BALL_PENALTY + assert int(key) == HeadMode.SEARCH_BALL_PENALTY elif key == "5": # Do a pattern which only looks in front of the robot - self.head_mode_msg.head_mode = HeadMode.LOOK_FRONT - assert int(key) == HeadMode.LOOK_FRONT + self.head_mode_msg.head_mode = HeadMode.SEARCH_FRONT + assert int(key) == HeadMode.SEARCH_FRONT elif key == "y": # kick left forward pass diff --git a/bitbots_motion/bitbots_head_mover/src/move_head.cpp b/bitbots_motion/bitbots_head_mover/src/move_head.cpp index 242887a6f..f4b690e65 100644 --- a/bitbots_motion/bitbots_head_mover/src/move_head.cpp +++ b/bitbots_motion/bitbots_head_mover/src/move_head.cpp @@ -717,14 +717,14 @@ class HeadMover { // Check if the head mode changed and if so, update the search pattern if (prev_head_mode_ != curr_head_mode) { switch (curr_head_mode) { - case bitbots_msgs::msg::HeadMode::BALL_MODE: // 0 + case bitbots_msgs::msg::HeadMode::SEARCH_BALL: // 0 pan_speed_ = params_.search_pattern.pan_speed; tilt_speed_ = params_.search_pattern.tilt_speed; pattern_ = generatePattern(params_.search_pattern.scan_lines, params_.search_pattern.pan_max[0], params_.search_pattern.pan_max[1], params_.search_pattern.tilt_max[0], params_.search_pattern.tilt_max[1], params_.search_pattern.reduce_last_scanline); break; - case bitbots_msgs::msg::HeadMode::BALL_MODE_PENALTY: // 11 + case bitbots_msgs::msg::HeadMode::SEARCH_BALL_PENALTY: // 11 pan_speed_ = params_.search_pattern_penalty.pan_speed; tilt_speed_ = params_.search_pattern_penalty.tilt_speed; pattern_ = @@ -733,7 +733,7 @@ class HeadMover { params_.search_pattern_penalty.tilt_max[1], params_.search_pattern.reduce_last_scanline); break; - case bitbots_msgs::msg::HeadMode::FIELD_FEATURES: // 3 + case bitbots_msgs::msg::HeadMode::SEARCH_FIELD_FEATURES: // 3 pan_speed_ = params_.search_pattern_field_features.pan_speed; tilt_speed_ = params_.search_pattern_field_features.tilt_speed; pattern_ = generatePattern( @@ -742,7 +742,7 @@ class HeadMover { params_.search_pattern_field_features.tilt_max[1], params_.search_pattern.reduce_last_scanline); break; - case bitbots_msgs::msg::HeadMode::LOOK_FRONT: // 13 + case bitbots_msgs::msg::HeadMode::SEARCH_FRONT: // 13 pan_speed_ = params_.front_search_pattern.pan_speed; tilt_speed_ = params_.front_search_pattern.tilt_speed; pattern_ = diff --git a/bitbots_msgs/msg/HeadMode.msg b/bitbots_msgs/msg/HeadMode.msg index 8d1603f91..7d2aa6d1e 100644 --- a/bitbots_msgs/msg/HeadMode.msg +++ b/bitbots_msgs/msg/HeadMode.msg @@ -2,17 +2,17 @@ # The body tells the head by this message what it shall do. # Search for Ball and track it if found -uint8 BALL_MODE=0 +uint8 SEARCH_BALL=0 # Look generally for all features on the field (ball, goals, corners, center point) -uint8 FIELD_FEATURES=1 +uint8 SEARCH_FIELD_FEATURES=1 # Simply look directly forward uint8 LOOK_FORWARD=2 #Don't move the head uint8 DONT_MOVE=3 # Ball Mode adapted for Penalty Kick -uint8 BALL_MODE_PENALTY = 4 +uint8 SEARCH_BALL_PENALTY = 4 # Do a pattern which only looks in front of the robot -uint8 LOOK_FRONT = 5 +uint8 SEARCH_FRONT = 5 uint8 head_mode From 42c22a4d0d90c3a7f1c998a68eb5704d59dfea80 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Wed, 15 Jan 2025 13:10:26 +0100 Subject: [PATCH 2/3] remove wrong comments --- bitbots_motion/bitbots_head_mover/src/move_head.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/bitbots_motion/bitbots_head_mover/src/move_head.cpp b/bitbots_motion/bitbots_head_mover/src/move_head.cpp index f4b690e65..0574de8aa 100644 --- a/bitbots_motion/bitbots_head_mover/src/move_head.cpp +++ b/bitbots_motion/bitbots_head_mover/src/move_head.cpp @@ -717,14 +717,14 @@ class HeadMover { // Check if the head mode changed and if so, update the search pattern if (prev_head_mode_ != curr_head_mode) { switch (curr_head_mode) { - case bitbots_msgs::msg::HeadMode::SEARCH_BALL: // 0 + case bitbots_msgs::msg::HeadMode::SEARCH_BALL: pan_speed_ = params_.search_pattern.pan_speed; tilt_speed_ = params_.search_pattern.tilt_speed; pattern_ = generatePattern(params_.search_pattern.scan_lines, params_.search_pattern.pan_max[0], params_.search_pattern.pan_max[1], params_.search_pattern.tilt_max[0], params_.search_pattern.tilt_max[1], params_.search_pattern.reduce_last_scanline); break; - case bitbots_msgs::msg::HeadMode::SEARCH_BALL_PENALTY: // 11 + case bitbots_msgs::msg::HeadMode::SEARCH_BALL_PENALTY: pan_speed_ = params_.search_pattern_penalty.pan_speed; tilt_speed_ = params_.search_pattern_penalty.tilt_speed; pattern_ = @@ -733,7 +733,7 @@ class HeadMover { params_.search_pattern_penalty.tilt_max[1], params_.search_pattern.reduce_last_scanline); break; - case bitbots_msgs::msg::HeadMode::SEARCH_FIELD_FEATURES: // 3 + case bitbots_msgs::msg::HeadMode::SEARCH_FIELD_FEATURES: pan_speed_ = params_.search_pattern_field_features.pan_speed; tilt_speed_ = params_.search_pattern_field_features.tilt_speed; pattern_ = generatePattern( @@ -742,7 +742,7 @@ class HeadMover { params_.search_pattern_field_features.tilt_max[1], params_.search_pattern.reduce_last_scanline); break; - case bitbots_msgs::msg::HeadMode::SEARCH_FRONT: // 13 + case bitbots_msgs::msg::HeadMode::SEARCH_FRONT: pan_speed_ = params_.front_search_pattern.pan_speed; tilt_speed_ = params_.front_search_pattern.tilt_speed; pattern_ = @@ -751,7 +751,7 @@ class HeadMover { params_.front_search_pattern.tilt_max[1], params_.search_pattern.reduce_last_scanline); break; - case bitbots_msgs::msg::HeadMode::LOOK_FORWARD: // 7 + case bitbots_msgs::msg::HeadMode::LOOK_FORWARD: pan_speed_ = params_.look_forward.pan_speed; tilt_speed_ = params_.look_forward.tilt_speed; pattern_ = generatePattern(params_.look_forward.scan_lines, params_.look_forward.pan_max[0], From 8df4f0b2958b0c2bdd521aaccbd5d0e3fd934b94 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Wed, 15 Jan 2025 14:00:48 +0100 Subject: [PATCH 3/3] refactor params for head movement --- .../bitbots_head_mover/config/head_config.yml | 397 +++++++++--------- .../bitbots_head_mover/src/move_head.cpp | 65 +-- 2 files changed, 236 insertions(+), 226 deletions(-) diff --git a/bitbots_motion/bitbots_head_mover/config/head_config.yml b/bitbots_motion/bitbots_head_mover/config/head_config.yml index ad975db9c..b74d3c3d7 100644 --- a/bitbots_motion/bitbots_head_mover/config/head_config.yml +++ b/bitbots_motion/bitbots_head_mover/config/head_config.yml @@ -44,204 +44,205 @@ move_head: bounds<>: [0.0, 8.0] # Search pattern for ball - search_pattern: - # search pattern speed - tilt_speed: - type: double - description: "Tilt speed for the search pattern" - default_value: 3.0 - - pan_speed: - type: double - description: "Pan speed for the search pattern" - default_value: 3.0 - - # Max values for the search pattern - pan_max: - type: double_array - default_value: [90.0, -90.0] - description: "Maximum pan values for the search pattern (in degrees)" - validation: - fixed_size<>: 2 - - tilt_max: - type: double_array - default_value: [-10.0, -60.0] - description: "Maximum tilt values for the search pattern (in degrees)" - validation: - fixed_size<>: 2 - - # Number of scan lines for the search pattern - scan_lines: - type: int - default_value: 2 - description: "Number of scan lines for the search pattern" - validation: - gt_eq<>: [1] - - # Reduces last scanline by that factor so that robot does not collide - reduce_last_scanline: - type: double - default_value: 0.2 - description: "Reduces last scanline by that factor so that robot does not collide" - validation: - bounds<>: [0.0, 1.0] - - search_pattern_penalty: - tilt_speed: - type: double - description: "Tilt speed for the search pattern" - default_value: 1.0 - - pan_speed: - type: double - description: "Pan speed for the search pattern" - default_value: 2.0 - - pan_max: - type: double_array - default_value: [-30.0, 30.0] - description: "Maximum pan values for the search pattern (in degrees)" - validation: - fixed_size<>: 2 - - tilt_max: - type: double_array - default_value: [-7.0, -30.0] - description: "Maximum tilt values for the search pattern (in degrees)" - validation: - fixed_size<>: 2 - - scan_lines: - type: int - default_value: 2 - description: "Number of scan horizontal lines for the search pattern" - validation: - gt_eq<>: [1] - - reduce_last_scanline: - type: double - default_value: 0.2 - description: "Reduces last scanline by that factor so that robot does not collide" - validation: - bounds<>: [0.0, 1.0] - - search_pattern_field_features: - tilt_speed: - type: double - description: "Tilt speed for the search pattern" - default_value: 3.0 - - pan_speed: - type: double - description: "Pan speed for the search pattern" - default_value: 3.0 - - pan_max: - type: double_array - default_value: [-90.0, 90.0] - description: "Maximum pan values for the search pattern (in degrees)" - validation: - fixed_size<>: 2 - - tilt_max: - type: double_array - default_value: [-10.0, -60.0] - description: "Maximum tilt values for the search pattern (in degrees)" - validation: - fixed_size<>: 2 - - scan_lines: - type: int - default_value: 2 - description: "Number of scan horizontal lines for the search pattern" - validation: - gt_eq<>: [1] - - reduce_last_scanline: - type: double - default_value: 0.2 - description: "Reduces last scanline by that factor so that robot does not collide" - validation: - bounds<>: [0.0, 1.0] - - front_search_pattern: - tilt_speed: - type: double - description: "Tilt speed for the search pattern" - default_value: 3.0 - - pan_speed: - type: double - description: "Pan speed for the search pattern" - default_value: 3.0 - - pan_max: - type: double_array - default_value: [ 0.0, 0.0 ] - description: "Maximum pan values for the search pattern (in degrees)" - validation: - fixed_size<>: 2 - - tilt_max: - type: double_array - default_value: [ -10.0, -70.0 ] - description: "Maximum tilt values for the search pattern (in degrees)" - validation: - fixed_size<>: 2 - - scan_lines: - type: int - default_value: 2 - description: "Number of scan horizontal lines for the search pattern" - validation: - gt_eq<>: [1] - - reduce_last_scanline: - type: double - default_value: 0.0 - description: "Reduces last scanline by that factor so that robot does not collide" - validation: - bounds<>: [0.0, 1.0] - - look_forward: - tilt_speed: - type: double - description: "Tilt speed for the search pattern" - default_value: 3.0 - - pan_speed: - type: double - description: "Pan speed for the search pattern" - default_value: 3.0 - - pan_max: - type: double_array - default_value: [ 0.0, 0.0 ] - description: "Maximum pan values for the search pattern (in degrees)" - validation: - fixed_size<>: 2 - - tilt_max: - type: double_array - default_value: [ -7.0, -7.0 ] - description: "Maximum tilt values for the search pattern (in degrees)" - validation: - fixed_size<>: 2 - - scan_lines: - type: int - default_value: 2 - description: "Number of scan horizontal lines for the search pattern" - validation: - gt_eq<>: [1] - - reduce_last_scanline: - type: double - default_value: 0.0 - description: "Reduces last scanline by that factor so that robot does not collide" - validation: - bounds<>: [0.0, 1.0] + search_patterns: + search_ball: + # search pattern speed + tilt_speed: + type: double + description: "Tilt speed for the search pattern" + default_value: 3.0 + + pan_speed: + type: double + description: "Pan speed for the search pattern" + default_value: 3.0 + + # Max values for the search pattern + pan_max: + type: double_array + default_value: [90.0, -90.0] + description: "Maximum pan values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + tilt_max: + type: double_array + default_value: [-10.0, -60.0] + description: "Maximum tilt values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + # Number of scan lines for the search pattern + scan_lines: + type: int + default_value: 2 + description: "Number of scan lines for the search pattern" + validation: + gt_eq<>: [1] + + # Reduces last scanline by that factor so that robot does not collide + reduce_last_scanline: + type: double + default_value: 0.2 + description: "Reduces last scanline by that factor so that robot does not collide" + validation: + bounds<>: [0.0, 1.0] + + search_ball_penalty: + tilt_speed: + type: double + description: "Tilt speed for the search pattern" + default_value: 1.0 + + pan_speed: + type: double + description: "Pan speed for the search pattern" + default_value: 2.0 + + pan_max: + type: double_array + default_value: [-30.0, 30.0] + description: "Maximum pan values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + tilt_max: + type: double_array + default_value: [-7.0, -30.0] + description: "Maximum tilt values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + scan_lines: + type: int + default_value: 2 + description: "Number of scan horizontal lines for the search pattern" + validation: + gt_eq<>: [1] + + reduce_last_scanline: + type: double + default_value: 0.2 + description: "Reduces last scanline by that factor so that robot does not collide" + validation: + bounds<>: [0.0, 1.0] + + search_field_features: + tilt_speed: + type: double + description: "Tilt speed for the search pattern" + default_value: 3.0 + + pan_speed: + type: double + description: "Pan speed for the search pattern" + default_value: 3.0 + + pan_max: + type: double_array + default_value: [-90.0, 90.0] + description: "Maximum pan values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + tilt_max: + type: double_array + default_value: [-10.0, -60.0] + description: "Maximum tilt values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + scan_lines: + type: int + default_value: 2 + description: "Number of scan horizontal lines for the search pattern" + validation: + gt_eq<>: [1] + + reduce_last_scanline: + type: double + default_value: 0.2 + description: "Reduces last scanline by that factor so that robot does not collide" + validation: + bounds<>: [0.0, 1.0] + + search_front: + tilt_speed: + type: double + description: "Tilt speed for the search pattern" + default_value: 3.0 + + pan_speed: + type: double + description: "Pan speed for the search pattern" + default_value: 3.0 + + pan_max: + type: double_array + default_value: [ 0.0, 0.0 ] + description: "Maximum pan values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + tilt_max: + type: double_array + default_value: [ -10.0, -70.0 ] + description: "Maximum tilt values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + scan_lines: + type: int + default_value: 2 + description: "Number of scan horizontal lines for the search pattern" + validation: + gt_eq<>: [1] + + reduce_last_scanline: + type: double + default_value: 0.0 + description: "Reduces last scanline by that factor so that robot does not collide" + validation: + bounds<>: [0.0, 1.0] + + look_forward: + tilt_speed: + type: double + description: "Tilt speed for the search pattern" + default_value: 3.0 + + pan_speed: + type: double + description: "Pan speed for the search pattern" + default_value: 3.0 + + pan_max: + type: double_array + default_value: [ 0.0, 0.0 ] + description: "Maximum pan values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + tilt_max: + type: double_array + default_value: [ -7.0, -7.0 ] + description: "Maximum tilt values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + scan_lines: + type: int + default_value: 2 + description: "Number of scan horizontal lines for the search pattern" + validation: + gt_eq<>: [1] + + reduce_last_scanline: + type: double + default_value: 0.0 + description: "Reduces last scanline by that factor so that robot does not collide" + validation: + bounds<>: [0.0, 1.0] position_reached_threshold: type: double diff --git a/bitbots_motion/bitbots_head_mover/src/move_head.cpp b/bitbots_motion/bitbots_head_mover/src/move_head.cpp index 0574de8aa..1a03edde7 100644 --- a/bitbots_motion/bitbots_head_mover/src/move_head.cpp +++ b/bitbots_motion/bitbots_head_mover/src/move_head.cpp @@ -718,45 +718,54 @@ class HeadMover { if (prev_head_mode_ != curr_head_mode) { switch (curr_head_mode) { case bitbots_msgs::msg::HeadMode::SEARCH_BALL: - pan_speed_ = params_.search_pattern.pan_speed; - tilt_speed_ = params_.search_pattern.tilt_speed; - pattern_ = generatePattern(params_.search_pattern.scan_lines, params_.search_pattern.pan_max[0], - params_.search_pattern.pan_max[1], params_.search_pattern.tilt_max[0], - params_.search_pattern.tilt_max[1], params_.search_pattern.reduce_last_scanline); + pan_speed_ = params_.search_patterns.search_ball.pan_speed; + tilt_speed_ = params_.search_patterns.search_ball.tilt_speed; + pattern_ = generatePattern( + params_.search_patterns.search_ball.scan_lines, params_.search_patterns.search_ball.pan_max[0], + params_.search_patterns.search_ball.pan_max[1], params_.search_patterns.search_ball.tilt_max[0], + params_.search_patterns.search_ball.tilt_max[1], + params_.search_patterns.search_ball.reduce_last_scanline); break; case bitbots_msgs::msg::HeadMode::SEARCH_BALL_PENALTY: - pan_speed_ = params_.search_pattern_penalty.pan_speed; - tilt_speed_ = params_.search_pattern_penalty.tilt_speed; - pattern_ = - generatePattern(params_.search_pattern_penalty.scan_lines, params_.search_pattern_penalty.pan_max[0], - params_.search_pattern_penalty.pan_max[1], params_.search_pattern_penalty.tilt_max[0], - params_.search_pattern_penalty.tilt_max[1], params_.search_pattern.reduce_last_scanline); + pan_speed_ = params_.search_patterns.search_ball_penalty.pan_speed; + tilt_speed_ = params_.search_patterns.search_ball_penalty.tilt_speed; + pattern_ = generatePattern(params_.search_patterns.search_ball_penalty.scan_lines, + params_.search_patterns.search_ball_penalty.pan_max[0], + params_.search_patterns.search_ball_penalty.pan_max[1], + params_.search_patterns.search_ball_penalty.tilt_max[0], + params_.search_patterns.search_ball_penalty.tilt_max[1], + params_.search_patterns.search_ball.reduce_last_scanline); break; case bitbots_msgs::msg::HeadMode::SEARCH_FIELD_FEATURES: - pan_speed_ = params_.search_pattern_field_features.pan_speed; - tilt_speed_ = params_.search_pattern_field_features.tilt_speed; - pattern_ = generatePattern( - params_.search_pattern_field_features.scan_lines, params_.search_pattern_field_features.pan_max[0], - params_.search_pattern_field_features.pan_max[1], params_.search_pattern_field_features.tilt_max[0], - params_.search_pattern_field_features.tilt_max[1], params_.search_pattern.reduce_last_scanline); + pan_speed_ = params_.search_patterns.search_field_features.pan_speed; + tilt_speed_ = params_.search_patterns.search_field_features.tilt_speed; + pattern_ = generatePattern(params_.search_patterns.search_field_features.scan_lines, + params_.search_patterns.search_field_features.pan_max[0], + params_.search_patterns.search_field_features.pan_max[1], + params_.search_patterns.search_field_features.tilt_max[0], + params_.search_patterns.search_field_features.tilt_max[1], + params_.search_patterns.search_ball.reduce_last_scanline); break; case bitbots_msgs::msg::HeadMode::SEARCH_FRONT: - pan_speed_ = params_.front_search_pattern.pan_speed; - tilt_speed_ = params_.front_search_pattern.tilt_speed; - pattern_ = - generatePattern(params_.front_search_pattern.scan_lines, params_.front_search_pattern.pan_max[0], - params_.front_search_pattern.pan_max[1], params_.front_search_pattern.tilt_max[0], - params_.front_search_pattern.tilt_max[1], params_.search_pattern.reduce_last_scanline); + pan_speed_ = params_.search_patterns.search_front.pan_speed; + tilt_speed_ = params_.search_patterns.search_front.tilt_speed; + pattern_ = generatePattern( + params_.search_patterns.search_front.scan_lines, params_.search_patterns.search_front.pan_max[0], + params_.search_patterns.search_front.pan_max[1], params_.search_patterns.search_front.tilt_max[0], + params_.search_patterns.search_front.tilt_max[1], + params_.search_patterns.search_ball.reduce_last_scanline); break; case bitbots_msgs::msg::HeadMode::LOOK_FORWARD: - pan_speed_ = params_.look_forward.pan_speed; - tilt_speed_ = params_.look_forward.tilt_speed; - pattern_ = generatePattern(params_.look_forward.scan_lines, params_.look_forward.pan_max[0], - params_.look_forward.pan_max[1], params_.look_forward.tilt_max[0], - params_.look_forward.tilt_max[1], params_.search_pattern.reduce_last_scanline); + pan_speed_ = params_.search_patterns.look_forward.pan_speed; + tilt_speed_ = params_.search_patterns.look_forward.tilt_speed; + pattern_ = generatePattern( + params_.search_patterns.look_forward.scan_lines, params_.search_patterns.look_forward.pan_max[0], + params_.search_patterns.look_forward.pan_max[1], params_.search_patterns.look_forward.tilt_max[0], + params_.search_patterns.look_forward.tilt_max[1], + params_.search_patterns.search_ball.reduce_last_scanline); break; default: