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()
