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)) 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 new file mode 100644 index 000000000..99dd4223c --- /dev/null +++ b/bitbots_body_behavior/config/body_parameters.yaml @@ -0,0 +1,595 @@ +body_behavior: + ros__parameters: + 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"] + describtion: "Possible roles in the team" + + + # 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 ] + 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 ] + descripton: "center" + validation: + bounds<>: [-1.0, 1.0] + + 1: + type: double_array + defaul_value: [ -0.45, 0.5 ] + description: "left" + 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 ] + descripton: "center" + validation: + bounds<>: [-1.0, 1.0] + + 1: + type: double_array + defaul_value: [ -0.45, 0.5 ] + description: "left" + 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 ] + deascription: "center" + validation: + bounds<>: [-1.0, 1.0] + + 1: + type: double_array + default_value: [ -0.2, 0.33 ] + description: "left" + 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 ] + description: "center" + validation: + bounds<>: [-1.0, 1.0] + + 1: + type: double_array + default_value: [ -0.085, 0.33 ] + description: "left" + validation: + bounds<>: [-1.0, 1.0] + + 2: + type: double_array + default_value: [ -0.085, -0.33 ] + description: "right" + 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." + 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" + 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: "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: + type: double, + 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." + 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 + # 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 + 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." + validation: + gt_eq<>: 0 + + # 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." + 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 + + + + + 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 + + + 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 + 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: + 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 # + ################## + 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 + + + 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" + }