From c95beb4a82882566939d4c4ccc1271705b885ba1 Mon Sep 17 00:00:00 2001 From: ayin21 Date: Wed, 26 Apr 2023 16:35:20 +0200 Subject: [PATCH 1/3] rewrite yaml --- .../config/body_parameters.yaml | 517 ++++++++++++++++++ .../config/head_parameters.yaml | 377 +++++++++++++ bitbots_head_behavior/package.xml | 1 + 3 files changed, 895 insertions(+) create mode 100644 bitbots_body_behavior/config/body_parameters.yaml create mode 100644 bitbots_head_behavior/config/head_parameters.yaml diff --git a/bitbots_body_behavior/config/body_parameters.yaml b/bitbots_body_behavior/config/body_parameters.yaml new file mode 100644 index 000000000..a4bff4a93 --- /dev/null +++ b/bitbots_body_behavior/config/body_parameters.yaml @@ -0,0 +1,517 @@ +body_behavior: + ros__parameters: + team_data_timeout: { + type: int, + default_value: 2, + description: "Data older than this is seen as non existent" + } + + + ball_max_covariance: { + type: double, + default_value: 0.5, + description: "minimal confidence to regard a ball of a team mate as valid" + } + + body: + roles: { + type: string_array, + default_value: ["goalie", "offense", "defense"], + } + + use_localization: { + type: bool, + default_value: true, + description: "When False, the behavior will use a simple fallback mode in which only detected image features are + used for decision making" + } + + # Position format: + # y + # ^ ______________________ + # | M | | | O + # | Y |_ -x, y | x, y _| P + # | G | | | | | P + # 0 + O | | ( ) | | G + # | A |_| | |_| O + # | L | -x,-y | x,-y | A + # | |__________|__________| L + # | + # +------------------+--------------> x + # 0 + # To be useful for different field sizes, use values in [-1, 1] for x and y + + role_positions: + goalie: { + type: double_array, + default_value: [ -0.95, 0.0 ], + } + defense: + passive: # passive: opponent has kickoff, active: we have kickoff + # position number 0 = center, 1 = left, 2 = right + 0: { + type: double_array, + defaul_value: [ -0.5, 0.0 ], + descripton: "center" + } + 1: { + type: double_array, + defaul_value: [ -0.45, 0.5 ], + description: "left" + } + 2: { + type: double_array, + defaul_value: [ -0.45, -0.5 ], + description: "right" + } + active: + 0: { + type: double_array, + defaul_value: [ -0.5, 0.0 ], + descripton: "center" + } + 1: { + type: double_array, + defaul_value: [ -0.45, 0.5 ], + description: "left" + } + 2: { + type: double_array, + defaul_value: [ -0.45, -0.5 ], + description: "right" + } + offense: + passive: + 0: { + type: double_array, + default_value: [ -0.27, 0.0 ], + deascription: "center" + } + 1: { + type: double_array, + default_value: [ -0.2, 0.33 ], + description: "left" + } + 2: { + type: double_array, + default_value: [ -0.2, -0.33 ], + description: "right" + } + active: + 0: { + type: double_array, + default_value: [ -0.1, 0.0 ], + description: "center" + } + 1: { + type: double_array, + default_value: [ -0.085, 0.33 ], + description: "left" + } + 2: { + type: double_array, + default_value: [ -0.085, -0.33 ], + description: "right" + } + + ready_wait_time: { + type: double, + default_value: 4.0, + description: "Time to wait in ready state before moving to role position to + give the localization time to converge." + } + + ball_lost_time: { + type: double, + default_value: 8.0, + description: "When the ball has not been seen for `ball_lost_time` seconds, + it is considered lost and will be searched" + } + + goal_alignment_orientation_threshold: { + type: double, + default_value: 20.0, + description: "The orientation threshold defining which range (in Degrees) is acceptable as aligned to the goal (in each direction)" + } + + goal_lost_time: { + type: double, + default_value: 30.0, + descripton: "When the goal has not been seen for `goal_lost_time` seconds, + it is considered lost and will be searched" + } + + ball_close_distance: { + type: double, + default_value: 1.5, + descripton: "When the ball is closer than `ball_close_distance` meters + it is in a reachable area of the robot" + } + + + ball_twist_precision_threshold: + x_sdev: { + type: double, + default_value: 0.3, + description: "the maximal allowed standard deviation of the ball twist." + } + y_sdev: { + type: double, + default_value: 0.3, + description: "the maximal allowed standard deviation of the ball twist." + } + + + ball_twist_lost_time: { + type: int, + default_value: 2, + description: "the duration after which a ball_twist is considered irrelevant." + } + + + ball_position_precision_threshold: + x_sdev: { + type: double, + default_value: 0.5, + description: "the maximal allowed standard deviation of the ball position on the x axis" + } + y_sdev: { + type: double, + default_value: 0.5, + description: "the maximal allowed standard deviation of the ball position on the y axis" + } + + # An area in which the ball can be kicked + # defined by min/max x/y values in meters which represent ball positions relative to base_footprint + # http://www.ros.org/reps/rep-0103.html#axis-orientation + kick_x_enter: { + type: double, + default_value: 0.24 + } + kick_x_leave: { + type: double, + default_value: 0.30 + } + kick_y_enter: { + type: double, + default_value: 0.14 + } + kick_y_leave: { + type: double, + default_value: 0.16 + } + + ball_dangerous_goal_radius: { + type: double, + default_value: 0.20, + description: "defines the radius around the goal (in form of a box) + in this area, the goalie will react to the ball. + the radius is the margin around the goal to both y and the positive x directions" + } + + + ball_dangerous_center: { + type: double, + default_value: 0.1, + description: "defines the area in which the goalie will not attempt to fall in front of the ball + this value represents the y displacement of the ball relative to the center of the robot" + } + + defensive_area: { + type: double, + default_value: 0.5, + description: "The defensive area is an area in which the players behave more defensive then usual + (defensive players are actively going to the ball and goalies move in the goal to be able to block the ball). + This affects the BallInDefensiveArea decision. + The area is described as portion of the field [0, 1] always measured from the own goal. + A value of 0.25 means, that the quarter in front of the own goal is the defensive area." + } + + block_position_goal_offset: { + type: double, + default_value: 0.15, + description: "This is the offset the goalie keeps to avoid crashes with poles when blocking balls. + The value describes the offset in meters from the goal line." + } + + block_position_gradient_factor: { + type: double, + default_value: 4.0, + description: "this factor defines how extreme the goalie reacts to a ball offset" + } + + # configurations for the use of bitbots_dynamic_kick package + dynamic_kick: + wait_time: { + type: double, + default_value: 10.0, + description: "time to wait for a dynamic_kick server" + } + topic: { + type: string, + default_value: "dynamic_kick", + description: "base topic under which an actionserver listens for KickAction messages", + } + + # the maximal allowed standard deviation of the localization pose. + localization_precision_threshold: + x_sdev: { + type: double, + default_value: 0.5, + } + y_sdev: { + type: double, + default_value: 0.5, + } + theta_sdev: { + type: double, + default_value: 0.6, + } + + + reorientation_duration: { + type: double, + default_value: 10.0, + description: "Duration for which the robot tries to orient itself, when the localization precision is low." + } + + reorientation_pause_duration: { + type: double, + default_value: 30.0, + description: "Duration for which the robot pauses between reorientation runs." + } + + pathfinding_position_threshold: { + type: double, + default_value: 0.3, + description: "minimal difference between the current and the last movebase goal to actually send a new goal." + } + pathfinding_orientation_threshold: { + type: double, + default_value: 10.0, + } + + goalpost_safety_distance: { + type: double, + default_value: 0.05, + description: "don't aim closer to goalpost than this" + } + + ball_far_approach_dist: { + type: double, + default_value: 0.5, + description: "Distance at which the ball is first approached before the ball obstacle is deactivated and we approach closer for the kick" + } + + ball_far_approach_position_thresh: { + type: double, + default_value: 0.2, + description: "Range in which the ball far approach point is counted as reached" + } + + ball_reapproach_dist: { + type: double, + default_value: 1.0, + description: "We reapproach the ball after it has moved further away than this distance. This includes movig to the far approach position. + Balls further away are also recognized as obstacles." + } + + ball_approach_dist: { + type: double, + default_value: 0.2, + description: "Distance at which the ball is normally approached" + } + + ball_reapproach_angle: { + type: double, + default_value: 1.2, + description: "Angle at which the ball is normally approached again" + } + + # topics the behavior subscribes to + ball_movement_subscribe_topic: 'ball_relative_movement' + + # The position where the supporter robot should place itself in order to accept a pass + pass_position_x: { + type: double, + default_value: 0.1, + } + pass_position_y: { + type: int, + default_value: 1, + } + supporter_max_x: { + type: int, + default_value: 4, + } + + max_kick_angle: { + type: double, + default_value: 1.4, + description: "maximal angle of a ball kick" + } # (radians) + + num_kick_angles: { + type: int, + default_value: 9, + description: "number of considered kick angles (uneven, otherwise the middle would be excluded)" + } + + penalty_kick_angle: { + type: double, + default_value: 0.44, + description: "the angle that the kick is done either to the left or right during penalty shoot out" + } + + # distance from center point, that the ball must be during an opponent kickoff to think that it moved + kickoff_min_ball_movement: 0.5 + + # dribble action + dribble_max_speed_x: { + type: double, + default_value: 0.25, + } + dribble_max_speed_y: { + type: double, + default_value: 0.08, + } + dribble_p: { + type: double, + default_value: 0.75, + } + dribble_accel_x: { + type: double, + default_value: 0.001, + } + + # dribble decision + dribble_orient_threshold: { + type: double, + default_value: 0.5, + } + dribble_goal_distance_threshold: { + type: double, + default_value: 1.5, + } + dribble_ball_distance_threshold: { + type: double, + default_value: 0.5, + } + dribble_kick_angle: { + type: double, + default_value: 0.6, + } + + kick_decision_smoothing: { + type: double, + default_value: 0.5, + } + + ################## + # costmap params # + ################## + + base_costmap_smoothing_sigma: { + type: double, + default_value: 0.3, + description: "sigma of gaussian blur applied to costmap" + } + + map_margin: { + type: double, + default_value: 1.0, + description: "margin that is added around the field size when creating the costmap (meters)" + } + + obstacle_costmap_smoothing_sigma: { + type: double, + default_value: 1.5, + description: "sigma of gaussian blur applied to obstacle costmap" + } + + goal_value: { + type: double, + default_value: 0.0, + description: "cost in the goal" + } + + goalpost_value: { + type: double, + default_value: 1.0, + description: "cost at a goalpost" + } + + corner_value: { + type: double, + default_value: 1.0, + description: "cost in a corner" + } + + in_field_value_our_side: { + type: double, + default_value: 1.5, + description: "start value on our side" + } + + keep_out_border: { + type: double, + default_value: 0.2, + description: "dangerous border area width in meters" + } + + obstacle_cost: { + type: double, + default_value: 2.0, + description: "cost of an obstacle" + } + + kick_cost_angular_range: { + type: double, + default_value: 0.5, + description: "angular range when estimating kick cost" + } + + kick_cost_kick_length: { + type: double, + default_value: 2.0, + description: "estimated kick length when estimating kick cost" + } + + time_to_ball_divider: { + type: double, + default_value: 25.0, + description: "parameters for time_to_ball estimation + divider of how often the time to ball is updated depending on update rate of the behavior + example: (125 = 1 per second, 250 = 1 per 2 seconds)" + } + + time_to_ball_cost_per_meter: { + type: double, + default_value: 7.0, + description: "7 seconds per meter when walking + # 6 sec per 1.57 rad (90 deg) = 3.82 sec per rad" + } + + time_to_ball_cost_start_angle: { + type: double, + default_value: 3.82, + description: "factor by which the difference in the starting angle and direction of the path (in rad) is weighted" + } + + time_to_ball_cost_goal_angle: { + type: double, + default_value: 3.82, + description: "same but for the goal angle" + } + + time_to_ball_cost_start_to_goal_angle: { + type: double, + default_value: 3.82, + description: "factor by which the difference in starting and goal angle is weighted (only if not turning to ball i.e. <1m)" + } + + time_to_ball_remember_time: { + type: double, + default_value: 1.0, + description: "seconds after which the time to ball is forgotten if a new path to the ball can not be calculated and evaluated" + } diff --git a/bitbots_head_behavior/config/head_parameters.yaml b/bitbots_head_behavior/config/head_parameters.yaml new file mode 100644 index 000000000..469c9abce --- /dev/null +++ b/bitbots_head_behavior/config/head_parameters.yaml @@ -0,0 +1,377 @@ +head_node: + head: + rosnode: { + type: string, + default_value: "head_behavior", + description: "Name of the head_behaviors ros-node" + } + + # Sane default values for some modules + defaults: + head_mode: { + type: int, + default_value: 0, + description: "Ball mode" + } + + # Max values for the head position + max_pan: { + type: double_array, + default_value: [-2.35, 2.35], + description: " Max values for the head position", + validation: { + fixed_size<>: 2 + } + } + max_tilt: { + type: double_array, + default_value: [-1.2, 0.2], + description: " Max values for the head position", + validation: { + fixed_size<>: 2 + } + } + + # TODO maybe keep this instead of the generated lines + # Search pattern for penalty + #search_pattern_penalty: + # - [0, -15] + # - [0, -30] + # - [30, -7] + # - [-30, -7] + + look_at: + tilt_speed: { + type: double, + default_value: 6.0, + } + pan_speed: { + type: double, + default_value: 6.0 + } + + # Search pattern for ball + search_pattern: + # Speed search pattern + tilt_speed: { + type: double, + default_value: 3.0 + } + pan_speed: { + type: double, + default_value: 3.0 + } + # Max values for the search pattern + pan_max: { + type: double_array, + default_value: [135.0, -135.0], + validation: { + fixed_size<>: 2 + } + } + tilt_max: { + type: double_array, + default_value: [-10.0, -60.0], + validation: { + fixed_size<>: 2 + } + } + + # Number of scan lines for the search pattern + scan_lines: { + type: string, + default_value: 2, + description: "Number of scan lines for the search pattern" + } + + # 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" + } + + + # Search pattern for penalty + search_pattern_penalty: + tilt_speed: { + type: double, + default_value: 1.0 + } + pan_speed: { + type: double, + default_value: 2.0, + } + pan_max: { + type: double_array, + default_value: [-30.0, 30.0], + validation: { + fixed_size<>: 2 + } + } + tilt_max: { + type: double_array, + default_value: [-7.0, -30.0], + validation: { + fixed_size<>: 2 + } + } + scan_lines: { + type: int, + default_value: 2, + } + reduce_last_scanline: { + type: double, + default_value: 0.2 + } + + # Search pattern for visual compass features search + visual_compass_features_pattern: + tilt_speed: { + type: double, + default_value: 1.0 + } + pan_speed: { + type: double, + default_value: 1.0 + } + pan_max: { + type: double_array, + default_value: [-45.0, 45.0], + validation: { + fixed_size<>: 2 + } + } + tilt_max: { + type: double_array, + default_value: [7.0, -7.0], + validation: { + fixed_size<>: 2 + } + } + scan_lines: { + type: int, + default_value: 2, + } + + search_pattern_goal: + tilt_speed: { + type: double, + default_value: 1.0 + } + pan_speed: { + type: double, + default_value: 1.5 + } + pan_max: { + type: double_array, + default_value: [135.0, -135.0], + validation: { + fixed_size<>: 2 + } + } + tilt_max: { + type: double_array, + default_value: [7.0, -7.0], + validation: { + fixed_size<>: 2 + } + } + scan_lines: { + type: int, + default_value: 2, + } + search_pattern_field_features: + tilt_speed: { + type: double, + default_value: 3.0 + } + pan_speed: { + type: double, + default_value: 3.0 + } + pan_max: { + type: double_array, + default_value: [-135.0, 135.0], + validation: { + fixed_size<>: 2 + } + } + tilt_max: { + type: double_array, + default_value: [-10.0, -60.0], + validation: { + fixed_size<>: 2 + } + } + scan_lines: { + type: int, + default-value: 2, + } + reduce_last_scanline: { + type: double, + default_value: 0.2 + } + + search_recent_ball: + tilt_speed: { + type: double, + default_value: 2.0 + } + pan_speed: { + type: double, + default_value: 2.0 + } + offset_pattern: { + type: string, + default_value: "[ + [ 0.0, 0.0], + [ 0.0, 20.0], + [ 0.0, -20.0], + [ 20.0,-20.0], + [ 20.0, 20.0], + [-20.0, 20.0], + [-20.0,-20.0], + [ 40.0,-20.0], + ]" + } + ball_search_time: { + type: double, + default_value: 20.0 + } + + front_search_pattern: + tilt_speed: { + type: double, + default_value: 3.0 + } + pan_speed: { + type: double, + default_value: 3.0 + } + pan_max: { + type: double_array, + default_value: [ 0.0, 0.0 ], + validation: { + fixed_size<>: 2 + } + } + tilt_max: { + type: double_array, + default_value: [ -10.0, -70.0 ], + validation: { + fixed_size<>: 2 + } + } + scan_lines: { + type: int, + default_value: 2, + } + reduce_last_scanline: { + type: double, + default_value: 0.0 + } + + # Values for the visual compass record pattern + record_pattern_scan_lines: { + type: int, + default_value: 2, + } + record_pattern_pan_max_left: { + type: double_array, + default_value: [-80.0, 0.0], + validation: { + fixed_size<>: 2 + } + } + record_pattern_pan_max_right: { + type: double_array, + default_value: [0.0, -80.0], + validation: { + fixed_size<>: 2 + } + } + record_pattern_tilt_max: { + type: double_array, + default_value: [14.0, 0.0], + validation: { + fixed_size<>: 2 + } + } + record_pattern_speed_tilt: { + type: double, + default_value: 1.0 + } + record_pattern_speed_pan: { + type: double, + default_value: 1.0 + } + interpolation_steps: { + type: int, + default_value: 4, + } + + # Visual compass ground truth trigger topic + visual_compass_trigger_topic: { + type: string, + default_value: '/visual_compass_ground_truth_trigger' + } + + + ball_tracking_min_pan_delta: { + type: double, + default_value: 1.5, + description: "These values describe the minimal required delta between current joint states and target + joint states in degree to reduce unnecessary movement due to noise in the detection of the Ball.", + + } + ball_tracking_min_tilt_delta: { + type: double, + default_value: 1.0 + description: "These values describe the minimal required delta between current joint states and target + joint states in degree to reduce unnecessary movement due to noise in the detection of the Ball.", + } + + + # Positions for static head modes + look_down_position: { + type: double_array, + default_value: [0.0, -65.0], + validation: { + fixed_size<>: 2 + } + } + look_forward_position: { + type: double_array, + default_value: [0.0, -7.0], + validation: { + fixed_size<>: 2 + } + } + look_up_position: { + type: double_array, + default_value: [0.0, 12.0], + validation: { + fixed_size<>: 2 + } + } + + + position_reached_threshold: { + type: double, + default_value: 5.0, + description: "Threshold (in degrees) when a head position is reached and + the next position will be triggered" + } + + # After `ball_lost_time` seconds, the ball is considered lost and will be searched + ball_lost_time: { + type: double, + default_value: 0.5, + description: "the time, when the ball is lost" + } + post_lost_time: { + type: double, + default_value: 1.0, + description: "time since the ball was lost" + } diff --git a/bitbots_head_behavior/package.xml b/bitbots_head_behavior/package.xml index 972db0ecd..20115c922 100644 --- a/bitbots_head_behavior/package.xml +++ b/bitbots_head_behavior/package.xml @@ -11,6 +11,7 @@ Hamburg Bit-Bots MIT + generate_parameter_library rclpy std_msgs bitbots_body_behavior From 644f752bf4014a7c7d7d216488fcfc6eed3345cb Mon Sep 17 00:00:00 2001 From: ayin21 Date: Tue, 9 Jan 2024 17:47:39 +0100 Subject: [PATCH 2/3] restuctured params and added description and bounds --- .../config/body_behavior.yaml | 16 +- .../config/body_parameters.yaml | 960 ++++++++++-------- 2 files changed, 520 insertions(+), 456 deletions(-) diff --git a/bitbots_body_behavior/config/body_behavior.yaml b/bitbots_body_behavior/config/body_behavior.yaml index 874497607..929deef1b 100644 --- a/bitbots_body_behavior/config/body_behavior.yaml +++ b/bitbots_body_behavior/config/body_behavior.yaml @@ -11,10 +11,6 @@ - "offense" - "defense" - # When False, the behavior will use a simple fallback mode in which only detected image features are - # used for decision making - use_localization: true - # Position format: # y # ^ ______________________ @@ -59,14 +55,9 @@ # it is considered lost and will be searched ball_lost_time: 3.0 - # The orientation threshold defining which range (in Degrees) is acceptable as aligned to the goal (in each direction) + # The orientation threshold defining which range (in Degrees) is acceptable as aligned to the path planning goal (in each direction) goal_alignment_orientation_threshold: 20.0 - # When the goal has not been seen for `goal_lost_time` seconds, - # it is considered lost and will be searched - goal_lost_time: 30.0 - - # When the ball is closer than `ball_close_distance` meters # it is in a reachable area of the robot ball_close_distance: 1.5 @@ -129,11 +120,6 @@ theta_sdev: 0.6 - # Duration for which the robot tries to orient itself, when the localization precision is low. - reorientation_duration: 10.0 - - # Duration for which the robot pauses between reorientation runs. - reorientation_pause_duration: 30.0 # minimal difference between the current and the last path planning goal to actually send a new goal. pathfinding_position_threshold: 0.3 diff --git a/bitbots_body_behavior/config/body_parameters.yaml b/bitbots_body_behavior/config/body_parameters.yaml index a4bff4a93..99dd4223c 100644 --- a/bitbots_body_behavior/config/body_parameters.yaml +++ b/bitbots_body_behavior/config/body_parameters.yaml @@ -1,30 +1,19 @@ body_behavior: ros__parameters: - team_data_timeout: { - type: int, - default_value: 2, - description: "Data older than this is seen as non existent" - } - - - ball_max_covariance: { - type: double, - default_value: 0.5, - description: "minimal confidence to regard a ball of a team mate as valid" - } - + team_data_timeout: + type: int + default_value: 2 + description: "Data send by the other robots older than this is seen as non existent" + ball_max_covariance: + type: double + default_value: 0.5 + description: "Minimal confidence to regard a ball of a team mate as valid" body: - roles: { - type: string_array, - default_value: ["goalie", "offense", "defense"], - } - - use_localization: { - type: bool, - default_value: true, - description: "When False, the behavior will use a simple fallback mode in which only detected image features are - used for decision making" - } + roles: + type: string_array + default_value: ["goalie", "offense", "defense"] + describtion: "Possible roles in the team" + # Position format: # y @@ -42,476 +31,565 @@ body_behavior: # To be useful for different field sizes, use values in [-1, 1] for x and y role_positions: - goalie: { - type: double_array, - default_value: [ -0.95, 0.0 ], - } + goalie: + type: double_array + default_value: [ -0.95, 0.0 ] + validation: + bounds<>: [-1.0, 1.0] + defense: passive: # passive: opponent has kickoff, active: we have kickoff # position number 0 = center, 1 = left, 2 = right - 0: { - type: double_array, - defaul_value: [ -0.5, 0.0 ], + 0: + type: double_array + defaul_value: [ -0.5, 0.0 ] descripton: "center" - } - 1: { - type: double_array, - defaul_value: [ -0.45, 0.5 ], + validation: + bounds<>: [-1.0, 1.0] + + 1: + type: double_array + defaul_value: [ -0.45, 0.5 ] description: "left" - } - 2: { - type: double_array, - defaul_value: [ -0.45, -0.5 ], + validation: + bounds<>: [-1.0, 1.0] + + 2: + type: double_array + defaul_value: [ -0.45, -0.5 ] description: "right" - } + validation: + bounds<>: [-1.0, 1.0] + active: - 0: { - type: double_array, - defaul_value: [ -0.5, 0.0 ], + 0: + type: double_array + defaul_value: [ -0.5, 0.0 ] descripton: "center" - } - 1: { - type: double_array, - defaul_value: [ -0.45, 0.5 ], + validation: + bounds<>: [-1.0, 1.0] + + 1: + type: double_array + defaul_value: [ -0.45, 0.5 ] description: "left" - } - 2: { - type: double_array, - defaul_value: [ -0.45, -0.5 ], + validation: + bounds<>: [-1.0, 1.0] + + 2: + type: double_array + defaul_value: [ -0.45, -0.5 ] description: "right" - } + validation: + bounds<>: [-1.0, 1.0] + offense: passive: - 0: { - type: double_array, - default_value: [ -0.27, 0.0 ], + 0: + type: double_array + default_value: [ -0.27, 0.0 ] deascription: "center" - } - 1: { - type: double_array, - default_value: [ -0.2, 0.33 ], + validation: + bounds<>: [-1.0, 1.0] + + 1: + type: double_array + default_value: [ -0.2, 0.33 ] description: "left" - } - 2: { - type: double_array, - default_value: [ -0.2, -0.33 ], + validation: + bounds<>: [-1.0, 1.0] + + 2: + type: double_array + default_value: [ -0.2, -0.33 ] description: "right" - } + validation: + bounds<>: [-1.0, 1.0] + active: - 0: { - type: double_array, - default_value: [ -0.1, 0.0 ], + 0: + type: double_array + default_value: [ -0.1, 0.0 ] description: "center" - } - 1: { - type: double_array, - default_value: [ -0.085, 0.33 ], + validation: + bounds<>: [-1.0, 1.0] + + 1: + type: double_array + default_value: [ -0.085, 0.33 ] description: "left" - } - 2: { - type: double_array, - default_value: [ -0.085, -0.33 ], + validation: + bounds<>: [-1.0, 1.0] + + 2: + type: double_array + default_value: [ -0.085, -0.33 ] description: "right" - } - - ready_wait_time: { - type: double, - default_value: 4.0, + validation: + bounds<>: [-1.0, 1.0] + + ready_wait_time: + type: double + default_value: 4.0 description: "Time to wait in ready state before moving to role position to give the localization time to converge." - } - - ball_lost_time: { - type: double, - default_value: 8.0, + validation: + gt_eq<>: 0 + + ball_lost_time: + type: double + default_value: 8.0 description: "When the ball has not been seen for `ball_lost_time` seconds, it is considered lost and will be searched" - } - - goal_alignment_orientation_threshold: { - type: double, - default_value: 20.0, - description: "The orientation threshold defining which range (in Degrees) is acceptable as aligned to the goal (in each direction)" - } - - goal_lost_time: { - type: double, - default_value: 30.0, - descripton: "When the goal has not been seen for `goal_lost_time` seconds, - it is considered lost and will be searched" - } - - ball_close_distance: { - type: double, - default_value: 1.5, + validation: + gt_eq<>: 0 + + goal_alignment_orientation_threshold: + type: double + default_value: 20.0 + description: "The orientation threshold defining which range (in Degrees) is acceptable as aligned to the path planning goal (in each direction)" + validation: + bounds<>: [0.0, 180.0] + + ball_close_distance: + type: double + default_value: 1.5 descripton: "When the ball is closer than `ball_close_distance` meters it is in a reachable area of the robot" - } + validation: + gt_eq<>: 0 - ball_twist_precision_threshold: - x_sdev: { - type: double, - default_value: 0.3, - description: "the maximal allowed standard deviation of the ball twist." - } - y_sdev: { - type: double, - default_value: 0.3, - description: "the maximal allowed standard deviation of the ball twist." - } - - - ball_twist_lost_time: { - type: int, - default_value: 2, - description: "the duration after which a ball_twist is considered irrelevant." - } + x_sdev: + type: double + default_value: 0.3 + description: "Maximum allowed standard deviation of the ball twist. (X Axis)" + y_sdev: + type: double + default_value: 0.3 + description: "Maximum allowed standard deviation of the ball twist. (Y Axis)" + + ball_twist_lost_time: + type: double + default_value: 2.0 + description: "The duration after which a ball_twist is considered irrelevant." + validation: + gt_eq<>: 0 ball_position_precision_threshold: - x_sdev: { + x_sdev: type: double, - default_value: 0.5, - description: "the maximal allowed standard deviation of the ball position on the x axis" - } - y_sdev: { + default_value: 0.5 + description: "The maximal allowed standard deviation of the ball position on the x axis." + validation: + gt_eq<>: 0 + + y_sdev: type: double, - default_value: 0.5, - description: "the maximal allowed standard deviation of the ball position on the y axis" - } + default_value: 0.5 + description: "The maximal allowed standard deviation of the ball position on the y axis." + validation: + gt_eq<>: 0 # An area in which the ball can be kicked # defined by min/max x/y values in meters which represent ball positions relative to base_footprint - # http://www.ros.org/reps/rep-0103.html#axis-orientation - kick_x_enter: { - type: double, - default_value: 0.24 - } - kick_x_leave: { - type: double, - default_value: 0.30 - } - kick_y_enter: { - type: double, - default_value: 0.14 - } - kick_y_leave: { - type: double, - default_value: 0.16 - } - - ball_dangerous_goal_radius: { - type: double, - default_value: 0.20, - description: "defines the radius around the goal (in form of a box) - in this area, the goalie will react to the ball. - the radius is the margin around the goal to both y and the positive x directions" - } - - - ball_dangerous_center: { - type: double, - default_value: 0.1, - description: "defines the area in which the goalie will not attempt to fall in front of the ball - this value represents the y displacement of the ball relative to the center of the robot" - } - - defensive_area: { - type: double, - default_value: 0.5, + # https://www.ros.org/reps/rep-0103.html#axis-orientation + # https://www.ros.org/reps/rep-0120.html + + kick_area: + x: + enter: + type: double + default_value: 0.24 + description: "The x direction of the area in wich the ball can be kicked. The ball needs to be closer than that to be considered in the kick area." + validation: + bounds<>: [0.0, 0.5] + leave: + type: double + default_value: 0.30 + description: "Once the ball was in the area it needs to be further than this on the x axix to leave it." + validation: + bounds<>: [0.0, 0.5] + y: + enter: + type: double + default_value: 0.14 + description: "The symmetric y direction of the area in wich the ball can be kicked. The ball needs to be closer than that to be considered in the kick area." + validation: + bounds<>: [0.0, 0.5] + leave: + type: double + default_value: 0.16 + description: "Once the ball was in the area it needs to be further than this on the symmetric y axix to leave it." + validation: + bounds<>: [0.0, 0.5] + + ball_dangerous: + goal_radius: + type: double + default_value: 2.0 + description: "Defines the radius in meter around the goal (in form of a box). + In this area, the goalie will react to the ball. + The radius is the margin around the goal to both y and the positive x directions." + validation: + gt_eq<>: 0 + center: + type: double + default_value: 0.1 + description: "Defines the area in which the goalie will not attempt to fall in front of the ball. + This value represents the y displacement of the ball relative to the center of the robot." + validation: + gt_eq<>: 0 + + + defensive_area: + type: double + default_value: 0.5 description: "The defensive area is an area in which the players behave more defensive then usual (defensive players are actively going to the ball and goalies move in the goal to be able to block the ball). This affects the BallInDefensiveArea decision. The area is described as portion of the field [0, 1] always measured from the own goal. A value of 0.25 means, that the quarter in front of the own goal is the defensive area." - } + validation: + bounds<>: [0.0, 1.0] - block_position_goal_offset: { - type: double, - default_value: 0.15, + block_position_goal_offset: + type: double + default_value: 0.15 description: "This is the offset the goalie keeps to avoid crashes with poles when blocking balls. The value describes the offset in meters from the goal line." - } + validation: + bounds<>: [0.0, 0.5] - block_position_gradient_factor: { - type: double, - default_value: 4.0, - description: "this factor defines how extreme the goalie reacts to a ball offset" - } + block_position_gradient_factor: + type: double + default_value: 4.0 + description: "This factor defines how extreme the goalie reacts to a ball offset." + validation: + gt_eq<>: 0 - # configurations for the use of bitbots_dynamic_kick package + # Configurations for the use of bitbots_dynamic_kick package. dynamic_kick: - wait_time: { - type: double, - default_value: 10.0, - description: "time to wait for a dynamic_kick server" - } - topic: { - type: string, - default_value: "dynamic_kick", - description: "base topic under which an actionserver listens for KickAction messages", - } - - # the maximal allowed standard deviation of the localization pose. + wait_time: + type: double + default_value: 10.0 + description: "Time to wait for a dynamic_kick server." + validation: + gt_eq<>: 0 + topic: + type: string + default_value: "dynamic_kick" + description: "Base topic under which an actionserver listens for KickAction messages." + validation: + not_empty<>: [] + + # The maximal allowed standard deviation of the localization pose. localization_precision_threshold: - x_sdev: { - type: double, - default_value: 0.5, - } - y_sdev: { - type: double, - default_value: 0.5, - } - theta_sdev: { - type: double, - default_value: 0.6, - } - - - reorientation_duration: { - type: double, - default_value: 10.0, - description: "Duration for which the robot tries to orient itself, when the localization precision is low." - } - - reorientation_pause_duration: { - type: double, - default_value: 30.0, - description: "Duration for which the robot pauses between reorientation runs." - } + x_sdev: + type: double + default_value: 0.5 + y_sdev: + type: double + default_value: 0.5 + theta_sdev: + type: double + default_value: 0.6 + + + + + goalpost_safety_distance: + type: double + default_value: 0.05 + description: "Don't aim closer to goalpost than this." + validation: + gt_eq<>: 0 + + ball_far_approach_dist: + type: double + default_value: 0.5 + description: "Distance at which the ball is first approached before the ball obstacle is deactivated and we approach closer for the kick." + validation: + gt_eq<>: 0 + + ball_far_approach_position_thresh: + type: double + default_value: 0.2 + description: "Range in which the ball far approach point is counted as reached" + validation: + gt_eq<>: 0 + + ball_reapproach_dist: + type: double + default_value: 1.0 + description: "We reapproach the ball after it has moved further away than this distance. This includes moving to the far approach position. + Balls further away are also recognized as obstacles." + validation: + gt_eq<>: 0 + + ball_approach_dist: + type: double + default_value: 0.2 + description: "Distance at which the ball is normally approached." + validation: + gt_eq<>: 0 + + ball_reapproach_angle: + type: double + default_value: 1.2 + description: "Angle at which the ball is normally approached again." + validation: + gt_eq<>: 0 - pathfinding_position_threshold: { - type: double, - default_value: 0.3, - description: "minimal difference between the current and the last movebase goal to actually send a new goal." - } - pathfinding_orientation_threshold: { - type: double, - default_value: 10.0, - } - - goalpost_safety_distance: { - type: double, - default_value: 0.05, - description: "don't aim closer to goalpost than this" - } - - ball_far_approach_dist: { - type: double, - default_value: 0.5, - description: "Distance at which the ball is first approached before the ball obstacle is deactivated and we approach closer for the kick" - } - - ball_far_approach_position_thresh: { - type: double, - default_value: 0.2, - description: "Range in which the ball far approach point is counted as reached" - } - - ball_reapproach_dist: { - type: double, - default_value: 1.0, - description: "We reapproach the ball after it has moved further away than this distance. This includes movig to the far approach position. - Balls further away are also recognized as obstacles." - } - - ball_approach_dist: { - type: double, - default_value: 0.2, - description: "Distance at which the ball is normally approached" - } - - ball_reapproach_angle: { - type: double, - default_value: 1.2, - description: "Angle at which the ball is normally approached again" - } - - # topics the behavior subscribes to - ball_movement_subscribe_topic: 'ball_relative_movement' + + ball_movement_subscribe_topic: + type: string + default_value: 'ball_relative_movement' + description: "The topic on which the ball movement is published. This is used to determine if the ball is moving." + validation: + not_empty<>: [] # The position where the supporter robot should place itself in order to accept a pass - pass_position_x: { - type: double, - default_value: 0.1, - } - pass_position_y: { - type: int, - default_value: 1, - } - supporter_max_x: { - type: int, - default_value: 4, - } - - max_kick_angle: { - type: double, - default_value: 1.4, - description: "maximal angle of a ball kick" - } # (radians) - - num_kick_angles: { - type: int, - default_value: 9, - description: "number of considered kick angles (uneven, otherwise the middle would be excluded)" - } - - penalty_kick_angle: { - type: double, - default_value: 0.44, - description: "the angle that the kick is done either to the left or right during penalty shoot out" - } - - # distance from center point, that the ball must be during an opponent kickoff to think that it moved - kickoff_min_ball_movement: 0.5 + pass_position: + x: + type: double + default_value: 0.1 + description: "The x position of the position where the supporter robot should place itself relative to the ball in order to accept a pass." + + y: + type: int + default_value: 1 + describtion: "The y position of the position where the supporter robot should place itself relative to the ball in order to accept a pass." + validation: + gt_eq<>: 0 + + supporter_max_x: + type: int + default_value: 4 + + + max_kick_angle: + type: double + default_value: 1.4 + description: "maximal angle of a ball kick (radians)." + validation: + bounds<>: [0.0, 3.14] + + num_kick_angles: + type: int + default_value: 9 + description: "Number of considered kick angles (uneven, otherwise the middle would be excluded)." + validation: + gt_eq<>: 0 + + + penalty_kick_angle: + type: double + default_value: 0.44 + description: "The angle that the kick is done either to the left or right during penalty shoot out (radians)." + validation: + bounds<>: [0.0, 3.14] + + + kickoff_min_ball_movement: + type: double + default_value: 0.5 + description: "Distance from center point, that the ball must be during an opponent kickoff to think that it moved." # dribble action - dribble_max_speed_x: { - type: double, - default_value: 0.25, - } - dribble_max_speed_y: { - type: double, - default_value: 0.08, - } - dribble_p: { - type: double, - default_value: 0.75, - } - dribble_accel_x: { - type: double, - default_value: 0.001, - } - - # dribble decision - dribble_orient_threshold: { - type: double, - default_value: 0.5, - } - dribble_goal_distance_threshold: { - type: double, - default_value: 1.5, - } - dribble_ball_distance_threshold: { - type: double, - default_value: 0.5, - } - dribble_kick_angle: { - type: double, - default_value: 0.6, - } - - kick_decision_smoothing: { - type: double, - default_value: 0.5, - } + dribble: + action: + max_speed: + x: + type: double + default_value: 0.3 + description: "Maximal speed in x direction (forward) when dribbling." + validation: + gt_eq<>: 0 + y: + type: double + default_value: 0.05 + description: "Maximal speed in y direction (sideways) when dribbling." + validation: + gt_eq<>: 0 + ball_heading_x_vel_zero_point: + type: double + default_value: 30 + description: "Ball angle at which we stop moving forward." + validation: + gt_eq<>: 0 + p: + type: double + default_value: 0.8 + description: "P value for dribble proportional controller." + validation: + gt_eq<>: 0 + accel: + x: + type: double + default_value: 0.01 + description: "Acceleration in x direction (forward) when dribbling." + validation: + gt_eq<>: 0 + y: + type: double + default_value: 0.01 + description: "Acceleration in y direction (sideways) when dribbling." + validation: + gt_eq<>: 0 + decision: + orient_threshold: + type: double + default_value: 0.5 + description: "The threshold for our orientation relative to the goal in radians to decide if we are sufficiently aligned to dribble." + validation: + gt_eq<>: 0 + goal_distance_threshold: + type: double + default_value: 1.5 + description: "The threshold for the distance to the goal in meters to decide if we are not to close to the goal to dribble." + validation: + gt_eq<>: 0 + ball_distance_threshold: + type: double + default_value: 0.5 + description: "The threshold for the distance to the ball in meters to decide if the ball is in a dribbleable position." + validation: + gt_eq<>: 0 + kick_angle_threshold: + type: double + default_value: 0.6 + description: "The threshold for the estimated best-kick-direktion. If the best kick direction is not in this range, we will kick instead." + validation: + gt_eq<>: 0 + + kick_decision_smoothing: + type: int + default_value: 5 + description: "Number of times that the ball matches the Near criterion in a row until it decides that it is Near. " + validation: + gt<>: 0 + ################## # costmap params # ################## - - base_costmap_smoothing_sigma: { - type: double, - default_value: 0.3, - description: "sigma of gaussian blur applied to costmap" - } - - map_margin: { - type: double, - default_value: 1.0, - description: "margin that is added around the field size when creating the costmap (meters)" - } - - obstacle_costmap_smoothing_sigma: { - type: double, - default_value: 1.5, - description: "sigma of gaussian blur applied to obstacle costmap" - } - - goal_value: { - type: double, - default_value: 0.0, - description: "cost in the goal" - } - - goalpost_value: { - type: double, - default_value: 1.0, - description: "cost at a goalpost" - } - - corner_value: { - type: double, - default_value: 1.0, - description: "cost in a corner" - } - - in_field_value_our_side: { - type: double, - default_value: 1.5, - description: "start value on our side" - } - - keep_out_border: { - type: double, - default_value: 0.2, - description: "dangerous border area width in meters" - } - - obstacle_cost: { - type: double, - default_value: 2.0, - description: "cost of an obstacle" - } - - kick_cost_angular_range: { - type: double, - default_value: 0.5, - description: "angular range when estimating kick cost" - } - - kick_cost_kick_length: { - type: double, - default_value: 2.0, - description: "estimated kick length when estimating kick cost" - } - - time_to_ball_divider: { - type: double, - default_value: 25.0, - description: "parameters for time_to_ball estimation - divider of how often the time to ball is updated depending on update rate of the behavior - example: (125 = 1 per second, 250 = 1 per 2 seconds)" - } - - time_to_ball_cost_per_meter: { - type: double, - default_value: 7.0, - description: "7 seconds per meter when walking - # 6 sec per 1.57 rad (90 deg) = 3.82 sec per rad" - } - - time_to_ball_cost_start_angle: { - type: double, - default_value: 3.82, - description: "factor by which the difference in the starting angle and direction of the path (in rad) is weighted" - } - - time_to_ball_cost_goal_angle: { - type: double, - default_value: 3.82, - description: "same but for the goal angle" - } - - time_to_ball_cost_start_to_goal_angle: { - type: double, - default_value: 3.82, - description: "factor by which the difference in starting and goal angle is weighted (only if not turning to ball i.e. <1m)" - } - - time_to_ball_remember_time: { - type: double, - default_value: 1.0, - description: "seconds after which the time to ball is forgotten if a new path to the ball can not be calculated and evaluated" - } + costmap: + base: + base_costmap_smoothing_sigma: + type: double + default_value: 0.3 + description: "Sigma of gaussian blur applied to costmap." + validation: + gt_eq<>: 0 + map_margin: + type: double + default_value: 1.0 + description: "Margin that is added around the field size when creating the costmap (meters)." + validation: + gt_eq<>: 0 + keep_out_border: + type: double + default_value: 0.2 + description: "Dangerous border area width in meters." + validation: + gt_eq<>: 0 + values: + goal: + type: double + default_value: 0.0 + description: "Cost in the goal." + validation: + gt_eq<>: 0 + goalpost: + type: double + default_value: 1.0 + description: "Cost at a goalpost." + validation: + gt_eq<>: 0 + corner: + type: double + default_value: 1.0 + description: "Cost in a corner." + validation: + gt_eq<>: 0 + in_field_value_our_side: + type: double + default_value: 1.5 + description: "Start value on our side." + validation: + gt_eq<>: 0 + + dynamic: + obstacle_costmap_smoothing_sigma: + type: double + default_value: 1.5 + description: "Sigma of gaussian blur applied to obstacle costmap." + validation: + gt_eq<>: 0 + obstacle_cost: + type: double + default_value: 2.0 + description: "Cost of an obstacle." + validation: + gt_eq<>: 0 + kick_cost: + angular_range: + type: double + default_value: 0.5 + description: "Angular range when estimating kick cost in a given direction (in radians)." + validation: + gt_eq<>: 0 + kick_length: + type: double + default_value: 2.0 + description: "Estimated kick length when estimating kick cost (in meter)." + validation: + gt_eq<>: 0 + + + time_to_ball: + divider: + type: double + default_value: 25.0 + description: "Parameters for time_to_ball estimation + divider of how often the time to ball is updated depending on update rate of the behavior + example: (125 = 1 per second, 250 = 1 per 2 seconds)." + validation: + gt_eq<>: 0 + remember_time: + type: double + default_value: 1.0 + description: "Seconds after which the time to ball is forgotten if a new path to the ball can not be calculated and evaluated." + validation: + gt_eq<>: 0 + cost: + per_meter: + type: double + default_value: 7.0 + description: "7 seconds per meter when walking + 6 sec per 1.57 rad (90 deg) = 3.82 sec per rad." + validation: + gt_eq<>: 0 + start_angle: + type: double + default_value: 3.82 + description: "Factor by which the difference in the starting angle and direction of the path (in rad) is weighted." + validation: + gt_eq<>: 0 + goal_angle: + type: double + default_value: 3.82 + description: "Factor by which the difference in the goal angle and direction of the path (in rad) is weighted." + validation: + gt_eq<>: 0 + start_to_goal_angle: + type: double + default_value: 3.82 + description: "Factor by which the difference in starting and goal angle is weighted (only if not turning to ball i.e. <1m)." + validation: + gt_eq<>: 0 + + + From 7fb22cb112654c4e5301017f9969c0c8b45980d2 Mon Sep 17 00:00:00 2001 From: ayin21 Date: Tue, 9 Jan 2024 17:48:36 +0100 Subject: [PATCH 3/3] capitalize comments --- .../bitbots_body_behavior/actions/go_to_block_position.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py index f8f45a638..c8f4924bb 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py @@ -47,6 +47,6 @@ def perform(self, reevaluate=False): self.blackboard.pathfinding.publish(pose_msg) def _stay_in_front_of_goal(self, y): - # keeps the y-values of the position in between of the goalposts. - # this ensures, that y is in [-self.blackboard.world_model.goal_width / 2, self.blackboard.world_model.goal_width / 2]. + # Keeps the y-values of the position in between of the goalposts. + # This ensures, that y is in [-self.blackboard.world_model.goal_width / 2, self.blackboard.world_model.goal_width / 2]. return max(-self.blackboard.world_model.goal_width / 2, min(self.blackboard.world_model.goal_width / 2, y))