def __init__(self, _): super(HeadDutyDecider, self).__init__() toggles = get_config()["Behaviour"]["Toggles"]["Head"] self.toggle_goal_vison_tracking = toggles["goalVisionTracking"] self.toggle_switch_ball_goal = toggles["switchBallGoalSearch"] config = get_config() self.confirm_time = config["Behaviour"]["Common"]["Search"]["confirmTime"] self.last_confirmd_goal = 0 self.fail_goal_counter = 0 self.ball_prio = 0 self.goal_prio = 0 self.trackjustball_aftergoal = False
def __init__(self, _): super(GoToCenterOfGoal, self).__init__() config = get_config() self.goalWidth = config["Field"]["goalWidth"] self.drehungsFaktor = 1.05 self.laufeSeit = None self.time = time.time()
def test_based_on_duty_y_center_for_tracking_is_set_correctly(self): behaviour_mock = mock.MagicMock() confirm = TrackBall() confirm.setup_internals(behaviour_mock, {}) nt = namedtuple("x", "y") nt.x = 0 nt.y = 0 connector = Connector(data={ DATA_KEY_BALL_FOUND: True, DATA_KEY_BALL_INFO: nt, "BallLastSeen": time.time(), "Pose": PyPose(), "Ipc": mock.MagicMock() }) connector.set_duty("Goalie") self.assertEquals("Goalie", connector.get_duty()) confirm.perform(connector) confirm.perform(connector) confirm.perform(connector) confirm.perform(connector) self.assertEqual(get_config()["Behaviour"]["Common"]["Tracking"]["yCenterGoalie"], confirm.b_center_goalie)
def __init__(self, _): super(PositionInGoal, self).__init__() config = get_config() goal_width = config["field"]["goal-width"] max_side_move = config["Behaviour"]["Goalie"]["maxSidewardMovement"] self.angle_threshold = config["Behaviour"]["Goalie"]["sidewardMoveAngleThreshold"] self.max_side = goal_width * max_side_move
def __init__(self, _): super(TurnAfterThrow, self).__init__() config = get_config() self.toggle_relocate_turn = config["Behaviour"]["Toggles"]["Goalie"]["relocateTurn"] self.anim_goalie_walkready = config["animations"]["motion"]["goalie-walkready"] self.accel_lst = [] self.starttime = time.time() self.vote_accel_z = False
def __init__(self, args="Normal"): super(GoToBall, self).__init__() config = get_config() self.camera_angle = config["Behaviour"]["Common"]["Camera"]["cameraAngle"] self.searching_turn_angular_absolute = config["Behaviour"]["Fieldie"]["searchingTurnAngularAbsolute"] self.max_kick_distance = config["Behaviour"]["Fieldie"]["kickDistance"] self.min_kick_distance = config["Behaviour"]["Fieldie"]["minKickDistance"] self.state = args
def __init__(self, args): super(Throw, self).__init__() config = get_config() self.richtung = args self.initializationTime = time.time() self.played = False self.left_animation = config["animations"]["goalie"]["throw_left"] self.middle_animation = config["animations"]["goalie"]["throw_middle"] self.right_animation = config["animations"]["goalie"]["throw_right"]
def perform(self, connector, reevaluate=False): # config laden config = get_config() # is last debug-call older than 1 second? (respectively older than G_DEBUGTIMETHRESHOLD) if config["Behaviour"]["Goalie"]["goalieDynamicDoLog"]: if int(int(time.time()) - self.G_lastcall) > self.G_DEBUGTIMETHRESOLD: self.G_lastcall = int(time.time()) # update last call time self.own_debug(connector) # --- Vars --- # rating rating = connector.use_raw_vision_info_capsule().get_ball_info("rating") # rating = connector.use_filtered_vision_info_capsule().get_simple_filtered_ball_info().rating # ball angle # ball_angle = connector.use_raw_vision_info_capsule().get_ball_angle() ball_angle = connector.use_filtered_vision_info_capsule().get_simple_filtered_ball_info().angle # get the estimated values from the BallDataInfoFilterModule uestimate, vestimate = connector.use_filtered_vision_info_capsule().get_uv_estimate() # Do nothing if rating is bad if rating > 30: return # Goal center (if seen) # Tuple: u,v # ToDo: File debuggen "FilteredVisionInfoCapsule.py" # , line 66, in get_complete_goal_seen if not (connector.use_filtered_vision_info_capsule().get_center_of_seen_goal() is None): self.goalCenter_old = tuple(self.goalCenter) self.goalCenter = connector.use_filtered_vision_info_capsule().get_center_of_seen_goal() else: self.goalCenter = tuple(self.goalCenter_old) # Distance to the goal center dist_goal_center = math.fabs(self.goalCenter[1]) # The bitbot shouldnt move to the post but stop at some threshold # (config["field"]["goal-width"] / 2.0) -> Half goal size # (config["field"]["goal-width"] / 20) -> 5% of the goal size self.WALKING_THRESHOLD = (config["field"]["goal-width"] / 2.0) - (config["field"]["goal-width"] / 20) # If ball is estimated to pass goal-line # Check the angle to the ball and correct position, if greater than threshold if math.fabs(ball_angle) < self.ACTIONANGLE: return # Check distance to the post and move or don't (if post is close or not) elif self.WALKING_THRESHOLD > dist_goal_center: if ball_angle < 0: return self.push(PlainWalkAction, [[WalkingCapsule.ZERO, WalkingCapsule.ZERO, WalkingCapsule.SLOW_SIDEWARDS_LEFT, 3]]) else: return self.push(PlainWalkAction, [[WalkingCapsule.ZERO, WalkingCapsule.ZERO, WalkingCapsule.SLOW_SIDEWARDS_RIGHT, 3]]) else: return
def __init__(self, _): super(GoToDutyPosition, self).__init__() config = get_config() self.length = config["field"]["length"] self.width = config["field"]["width"] self.goalie_position = config["Behaviour"]["Common"]["Positions"]["goalie"] self.teamplayer_position = config["Behaviour"]["Common"]["Positions"]["teamPlayer"] self.defender_position = config["Behaviour"]["Common"]["Positions"]["defender"] self.center_position = config["Behaviour"]["Common"]["Positions"]["center"] self.threshold = config["Behaviour"]["Common"]["positioningThreshold"]
def test_integration(self): data = {} self.connector = Connector(data) self.connector.walking_capsule().start_walking(forward_key=WalkingCapsule.MEDIUM_FORWARD, sidewards_key=WalkingCapsule.FAST_SIDEWARDS_RIGHT) conf = get_config()["Behaviour"]["Common"]["Walking"] self.assertEquals(conf[WalkingCapsule.MEDIUM_FORWARD], data["Walking.Forward"]) self.assertEquals(conf[WalkingCapsule.FAST_SIDEWARDS_RIGHT], data["Walking.Sideward"]) self.assertEquals(True, data["Walking.Active"])
def __init__(self, _): super(GoalieMovement, self).__init__() config = get_config()["Behaviour"] self.toggle_goalie_becomes_fieldie = config["Toggles"]["Goalie"]["goalieGoFieldie"] self.toggle_goalie_go_to_ball = config["Toggles"]["Goalie"]["goalieGoToBall"] self.toggle_goalie_position_in_goal = config["Toggles"]["Goalie"]["walkInGoal"] self.toggle_goalie_check_direction = config["Toggles"]["Goalie"]["checkDirection"] self.go_to_ball_velocity = config["Goalie"]["goToBallVelocity"] self.go_to_ball_u = config["Goalie"]["goToBallu"] self.sideward_move_u_threshold = config["Goalie"]["sidewardMoveUThreshold"] self.direction_angle_threshold = config["Goalie"]["directionAngleThreshold"]
def start(self, data): config = get_config() self.max_ball_distance = config['field']['length'] + 2000 config = config["vision"] self.export_frame_skip = config["export_every_n_th_frame"] self.ball_pos_is_ball_footpoint = config["ball_pos_is_ball_footprint"] color_config = self.load_color_config(config["VISION_COLOR_CONFIG"]) tr_config = config["vision-thresholds"] width, heigth = data[DATA_KEY_CAMERA_RESOLUTION] self.use_kinematic_horizon = config["use_kinematic_horizon"] self.vision = RobotVision(tr_config["green_y"], tr_config["green_u"], tr_config["green_v"], tr_config["green_dynamic"], width, heigth, config["vision-ignore_goals_out_of_field"]) self.vision.set_color_config(color_config) self.vision.set_ball_enabled(config["vision-ball_enabled"]) self.vision.set_goals_enabled(config["vision-goals_enabled"]) self.vision.set_lines_enabled(config["vision-lines_enabled"]) self.vision.set_pylons_enabled(config["vision-pylons_enabled"]) self.vision.set_shape_vectors_enabled( config["vision-shape_vectors_enabled"]) self.vision.set_team_marker_enabled(config["vision-team_marker_enabled"]) self.vision.set_is_picture_inverted(config["invertetPicture"]) self.vision.set_ball_pos_is_ball_footpoint(self.ball_pos_is_ball_footpoint) self.vision.set_rgb_step_factor(config["RGB_STEP_FACTOR"]) self.vision.set_b_w_debug_image(config["SEND_BW_DEBUG"]) self.vision.set_min_intensity(config["vision-intensity-min"]) self.vision.set_max_intensity(config["vision-intensity-max"]) # setzen des erwarteten bildformates self.vision.set_image_format(data[DATA_KEY_IMAGE_FORMAT]) self.transformer = Transformer() data[DATA_KEY_TRANSFORMER] = self.transformer self.transformer.set_camera_angle(config["CameraAngle"]) self.vision.set_camera_exposure_callback(data["CameraExposureCalback"]) data[DATA_KEY_CAMERA_EXPOSURE_CALLBACK] = None data[DATA_KEY_SUPPORT_LEG] = 0 data[DATA_KEY_OBSTACLE_FOUND] = False # obstacle infos --- veraltet # self.extract_obstacle_infos(data, self.vision.obstacles) data[DATA_KEY_GOAL_FOUND] = 0 data[DATA_KEY_GOAL_INFO] = None data[DATA_KEY_OBSTACLE_FOUND] = 0 data[DATA_KEY_OBSTACLE_INFO] = [] #if do_not_override: if os.path.exists("/home/darwin"): files=os.listdir("/home/darwin") while "frame-%04d.yuv" % self.idx in files: self.idx = self.idx + 1
def init_robot_and_pose(self, roll_angle=0, pitch_angle=0): pose = PyPose() config = get_config() config["RobotTypeName"] = "Hambot" internal_camera_angle = config[config["RobotTypeName"]]["Head"][3]["rpy"][1] robot = Robot() pose.belly_pitch.position = -pitch_angle pose.belly_roll.position = -roll_angle assert pose.belly_roll.position == -roll_angle assert pose.belly_pitch.position == -pitch_angle robot.update(pose) return (robot, pose, internal_camera_angle)
def __init__(self, _): super(AbstractSearchAndConfirm, self).__init__(_) self.set_confirmed = None self.get_started_confirm_time = None self.set_started_confirm_time = None self.unset_started_confirm_time = None self.object_seen = None self.object_last_seen = None self.fr = True config = get_config() self.fail_counter = 0 self.confirm_time = config["Behaviour"]["Common"]["Search"]["confirmTime"] self.track_ball_lost_time = config["Behaviour"]["Common"]["Tracking"]["trackBallLost"] self.ball_fail_conter_max = config["Behaviour"]["Common"]["Tracking"]["ballFailCounterMax"]
def __init__(self): debug_conf = get_config()["debugui"] self.dump_data_in_plain = debug_conf["DUMP_RECEIVING_DATA_IN_PLAIN"] self.dump_data_file_path = debug_conf["DUMP_FILE_PATH"] self.clear_dump_file_on_startup = debug_conf["CLEAR_DUMP_FILE_PATH_ON_STARTUP"] # Add the dump file if it not exists by now self.setup_dump_file() # Add the Filter Chain Elements for Specific Debug Messages self.filter_chain_elements = [ ball_info_fce, ball_info_data_filter_fce, goalie_line_intersection_fce, goal_info_fce ]
def __init__(self, _): super(DynamicKick, self).__init__() config = get_config() self.walkready_anim = config["animations"]["motion"]["walkready"] self.run = -1 self.x = 0 self.y1 = 0 self.y2 = 0 self.root = 0 self.end_joint = None self.angle_task_joints = None self.ignore_joints = None self.start = None self.ende = None self.lr = None self.pose = None
def __init__(self, debug=None): if not debug: self.debug = Scope("Nice") else: self.debug = debug.sub("Nice") config = get_config() if config['nice']: try: self.nice_ok = True self.level = self.get_nice() self.__test_nice() except: # behandlung um intern consistent zu bleiben self.nice_ok = False raise # fehler weiterwerfen else: self.debug.log("Nice ist in der Config deaktiviert") self.nice_ok = False
def start(self, data): self.transformer = data[DATA_KEY_TRANSFORMER] # initialisirungen data[DATA_KEY_BALL_FOUND] = False data[DATA_KEY_BALL_LAST_SEEN] = 0 config = get_config() self.max_ball_distance = config["vision"]["max_distance"] self.ball_radius = config["vision"]["DEFAULT_RADIUS"] self.ball_raidus_max = config["vision"]["ball_radius_max"] self.ball_radius_min = config["vision"]["ball_radius_min"] self.cam_winkel = math.radians(config["vision"]['CameraAngle']) # todo was 3.1 self.ball_pos_is_ball_footpoint = config["vision"]["ball_pos_is_ball_footprint"] self.rating_max = config["vision"]["rating_max"] self.number_rated_candidates = config["vision"]["number_rated_candidates"] self.max_distance_to_motor = config["vision"]["max_distance_to_motor"] self.activate_orange_ball_hack = config["vision"]["orange_ball_hack"]
def start(self, data): self.transformer = data[DATA_KEY_TRANSFORMER] # initialisirungen data[DATA_KEY_GOAL_FOUND] = False data[DATA_KEY_ANY_GOALPOST_LAST_SEEN] = 0 config = get_config() self.max_goalpost_distance = config["field"]["length"] + 2000 # TODO: warum plus 2000? #vlt um sicher zu sein self.goalpost_width = config["vision"]["GOALPOST_WIDTH"] self.goalpost_height = config["vision"]["GOALPOST_HEIGHT"] self.goalpost_width_min = config["vision"]["goalpost_width_min"] self.goalpost_width_max = config["vision"]["goalpost_width_max"] self.goalpost_height_min = config["vision"]["goalpost_height_min"] self.goalpost_height_max = config["vision"]["goalpost_height_max"] self.goal_width = config["field"]["goal-width"] self.goal_width_tolerance = config["vision"]["goal_width_tolerance"] self.cam_winkel = math.radians(config["vision"]["CameraAngle"]) # todo was 3.1 self.focal_length = config["vision"]["FocalLength"]
def look_at_with_offset(connector, pan_offset, target): common_behaviour_config = get_config()["Behaviour"]["Common"] pan_speed = common_behaviour_config["Search"]["maxPanSpeedSearch"] tilt_speed = common_behaviour_config["Search"]["maxTiltSpeedSearch"] max_pan_angle = common_behaviour_config["Camera"]["maxPan"] # max angle to look down (must be positive) max_down_angle = common_behaviour_config["Camera"]["minTilt"] # max angle to look up (must be positive) max_up_angle = common_behaviour_config["Camera"]["maxTilt"] local_goal_model = connector.filtered_vision_capsule().get_local_goal_model() if target == "Ball": target_pan, target_tilt = get_pantilt_from_uv(local_goal_model.get_ball_position()[0], local_goal_model.get_ball_position()[1]) elif target == "EnemyGoal": target_pan, target_tilt = get_pantilt_from_uv(local_goal_model.get_opp_goal()[0], local_goal_model.get_opp_goal()[1]) elif target == "OwnGoal": target_pan, target_tilt = get_pantilt_from_uv(local_goal_model.get_own_goal()[0], local_goal_model.get_own_goal()[1]) else: raise ValueError() if math.fabs(target_pan + pan_offset) < max_pan_angle: target_pan += pan_offset elif target_pan + pan_offset < 0: target_pan = - max_pan_angle else: target_pan = max_pan_angle if target_tilt > max_up_angle: target_tilt = max_up_angle elif target_tilt < max_down_angle: target_tilt = max_down_angle pose = connector.get_pose() ipc = connector.get_ipc() pose.head_pan.goal = target_pan pose.head_tilt.goal = target_tilt pose.head_pan.speed = pan_speed pose.head_tilt.speed = tilt_speed connector.set_pose(pose)
def test_chain_masspoints(self): print "### Masspoint ###" config = get_config() config["RobotTypeName"] = "Darwin" robot = Robot() task = KinematicTask(robot) r_chain = task.create_cog_chain(3) l_chain = task.create_cog_chain(4) robot.update(PyPose()) task.update_chain(l_chain, 3) l_cmp = l_chain.get_joint(6).get_chain_masspoint() task.update_chain(r_chain, 3) r_cmp = r_chain.get_joint(6).get_chain_masspoint() r_cmp[1] = -r_cmp[1] diff = l_cmp - r_cmp diff = np.where(diff < 0, -diff, diff) max = diff.max() if(not diff.max() < 1e-5): print "Chain masspoints differ:\n%s\n%s\t%s" % (diff, l_cmp,r_cmp) print "\n FALSE DIFF\n %s \n" % diff
def test_maspoint_simple_r(self): print "### Masspoint simple right ###" config = get_config() config["RobotTypeName"] = "Darwin" robot = Robot() task = KinematicTask(robot) r_chain = task.create_cog_chain(3) l_chain = task.create_cog_chain(4) robot.update(PyPose()) cog=robot.get_centre_of_gravity() lfi = robot.get_joint_by_id(35).get_chain_matrix(inverse=True) rfi = robot.get_joint_by_id(34).get_chain_matrix(inverse=True) rcog = np.dot(rfi, cog) lcog = np.dot(lfi, cog) task.update_chain(r_chain, 3) r_cmp = r_chain.get_joint(6).get_chain_masspoint() diff = r_cmp - rcog[0:3] print diff
def __init__(self): self.config = get_config()
.. moduleauthor:: Martin Poppinga <*****@*****.**> History: * 29.11.13: Created (Martin Poppinga) """ from bitbots.modules.abstract.abstract_decision_module import AbstractDecisionModule from bitbots.modules.behaviour.body.actions.go_to_ball_intelligent import GoToBallIntelligent from bitbots.modules.behaviour.body.decisions.common.stands_correct_decision import StandsCorrectDecision from bitbots.modules.behaviour.body.decisions.penalty.penalty_first_kick import PenaltyFirstKick from bitbots.util import get_config from bitbots.modules.behaviour.body.decisions.common.kick_decision import KickDecisionPenaltyKick from bitbots.modules.behaviour.body.decisions.common.kick_decision import KickDecisionThrowIn from bitbots.modules.behaviour.body.actions.go_to_ball import GoToBallPenaltykick config = get_config() class AbstractCloseBall(AbstractDecisionModule): """ Test if the ball is in kick distance """ def __init__(self, _): super(AbstractCloseBall, self).__init__() self.last_goalie_dist = 0 self.last_goalie_dist_time = 0 self.max_kick_distance = config["Behaviour"]["Fieldie"]["kickDistance"] self.min_kick_distance = config["Behaviour"]["Fieldie"]["minKickDistance"] def perform(self, connector, reevaluate=False):
def __init__(self, _): super(OneTimeKickerDecision, self).__init__() config = get_config() self.reset_time = config["Behaviour"]["OneTimeKicker"]["resetTime"]
def __init__(self, _): super(FieldieSearchDecision, self).__init__() config = get_config() self.start_time = time.time() self.turn_wait_time = config["Behaviour"]["Common"]["Search"]["turnWaitTime"]
# from debugui.imageview import ImageView import bitbots.glibevent import gobject import copy # from functions import make_ui_proxy import traceback from debuguineu.functions import find from debuguineu.plaindump import DebugPlainDumper config = get_config()["debugui"] class DataItem(object): """ A Dataitem represents a piece of information send by a robot """ __slots__ = ("name", "type", "value") def __init__(self, name, type, value): self.name = name self.type = type self.value = value
def __init__(self): super(KickOffRoleDecider, self).__init__() config = get_config() self.decision_distance = config["Behaviour"]["Fieldie"]["KickOff"]["roleDecisionDistance"]
def test_Darwin_config(self): print "### Darwin ###" config = get_config() config["RobotTypeName"] = "Darwin" robot = Robot() larm = robot.get_l_arm_chain()
def test_GOAL_config(self): print "### Goal ###" config = get_config() config["RobotTypeName"] = "Hambot" robot = Robot() larm = robot.get_l_arm_chain()