def get_driving_displacement(self, uniqueID): ''' Fetch the total displacement ''' dx = 0.0 dy = 0.0 dtheta = 0.0 try: if not self._displacement.has_key(uniqueID): self._displacement[uniqueID] = [0.0, 0.0, 0.0] dx = copy.copy(self._displacement[uniqueID][0]) dy = copy.copy(self._displacement[uniqueID][1]) dtheta = copy.copy(self._displacement[uniqueID][2]) self._displacement[uniqueID][0] = 0.0 self._displacement[uniqueID][1] = 0.0 self._displacement[uniqueID][2] = 0.0 trace('get driving displacement = %6.2f, %6.2f, %6.2f', dx, dy, dtheta) except: traceError('Failed to get driving displacement') print traceback.format_exc() print sys.exc_info()[0] return dx, dy, dtheta
def __init__(self, compassObject): ''' Initialize motors class Steps: - Open serial ports for driving motors - Open serial ports for ball handling motors ''' try: self._motor_ports = [] self._robot_speed_x = 0.0 self._robot_speed_y = 0.0 self._robot_speed_theta = 0.0 self._displacement = {} self._vel_motor_left = 0.0 self._vel_motor_right = 0.0 self._vel_motor_rear = 0.0 self._ball_speed_left = 0.0 self._ball_speed_right = 0.0 self._timestamp = datetime.datetime.now() self._displacementTimestamp = datetime.datetime.now() self._compass = compassObject self.ball_possession_status = False self._bhAngle = 0 self._init_services() trace('Motors class initialized') except: traceError('failed to initialize cMotors')
def _calc_angle(self, dX, dY): ''' Calculate the angle between two deltas ''' try: th = 0.0 radius = self._calc_radius(dX, dY) if (dY < 0.0): if (dX == 0.0): th = pi / 2.0 else: th = arccos(-dX / radius) elif (dY > 0.0): if (dX == 0.0): th = 3.0 / 2.0 * pi else: th = 2.0 * pi - arccos(-dX / radius) elif (dY == 0.0): if (dX > 0.0): th = 0.0 else: th = pi except: traceError('failed to calculate angle') return th
def _trigger_event_on_ball_possession_change(self): ''' call the ros service to update if there is a state change in ball possession. there can be two possible states: ('has ball' or 'has no ball') ''' try: current_ball_possession_status = self.do_robot_have_balls() except: traceError('Cannot get current ball possession')
def activate_ball_handling(self, speed_left=0, speed_right=0): 'set the speed of ball handling cMotors' try: self._ball_speed_left = speed_left self._ball_speed_right = speed_right trace('activated ball handling') except: traceError('Failed to set active ball handling')
def stop_robot(self): 'stops the robot by sending speed 0' try: self._vel_motor_left = 0.0 self._vel_motor_right = 0.0 self._vel_motor_rear = 0.0 trace('robot stopped') except: traceError('Cannot stop robot')
def _map_0_to_360(self, angle): ret_val = angle try: while (ret_val < 0): ret_val += 360.0 while (ret_val > 360.0): ret_val -= 360.0 except: traceError('failed to map angle') return ret_val
def _project_angle_mpi_pi(self, angle): tmp_angle = copy(angle) try: while tmp_angle < -pi: tmp_angle += 2.0 * pi while tmp_angle > pi: tmp_angle -= 2 * pi except: traceError('failed to project angle') return tmp_angle
def _calc_angle(self): 'Calculate time difference in seconds' try: time_now = datetime.datetime.now() timediff_sec = (time_now - self._timestamp).microseconds / 1000000.0 'Calculate new angle with old velocity before updating' self._angle += self._vel_angle * timediff_sec 'Update own timestamp' self._timestamp = time_now except: traceError('failed to calculate angle')
def __init__(self): ''' Initialize cCompass class Steps: - Open serial communication - Create cyclic timer to read cCompass value - Start timer ''' try: self._angle = 0.0 self._vel_angle = 0.0 self._timestamp = datetime.datetime.now() except: traceError('Failed to initialize Compass')
def _set_speed(self, motor_left_vel, motor_right_vel, motor_rear_vel): 'set the speed individually to cMotors left,right and rear' try: self._vel_motor_left = motor_left_vel self._vel_motor_right = motor_right_vel self._vel_motor_rear = motor_rear_vel trace('set speed = %6.2f, %6.2f, %6.2f', motor_left_vel, motor_right_vel, motor_rear_vel) trace('_set_speed finished') except: traceError('failed to set_speed')
def setKickSpeed(self, request): 'adjusts the shoot height and triggers the shooter with the set speed' response = s_peripheralsInterface_setKickSpeedResponse() try: self._shoot_speed = float(request.kick_speed) trace('shoot speed: %8.4f' % (self._shoot_speed)) resp = self._sim_shoot_srv(self._shoot_speed * cKicker.BALL_SIM_FACTOR) except: traceError('failed to shoot with shoot speed: %8.4f' % (self._shoot_speed)) return response
def get_driving_speed(self): ''' Fetch robot speed in meters per second ''' vx = 0.0 vy = 0.0 vtheta = 0.0 try: vx = self._robot_speed_x vy = self._robot_speed_y vtheta = self._robot_speed_theta trace('get driving speed=%6.2f, %6.2f, %6.2f', vx, vy, vtheta) except: traceError('Failed to get driving speed') return vx, vy, vtheta
def _init_services(self): 'Initialize services that peripheralsInterface is providing' try: self._srv_get_ballHandlers_angle = rospy.Service( 's_get_ballHandlers_angle', s_get_ballHandlers_angle, self._getBallHandlersAngle) self._srv_set_ballHandlers_angle = rospy.Service( 's_set_ballHandlers_angle', s_set_ballHandlers_angle, self._setBallHandlersAngle) self._srv_disable_ballHandlers = rospy.Service( 's_disable_ballHandlers', s_disable_ballHandlers, self._disableBallHandlers) self._srv_enable_ballHandlers = rospy.Service( 's_enable_ballHandlers', s_enable_ballHandlers, self._enableBallHandlers) except: traceError('Failed to init own services') print traceback.format_exc() print sys.exc_info()[0]
def move_robot(self, x_velocity, y_velocity, theta_velocity): 'moves the robot by distributing the speed to drive wheels in varying ratio based on the input' try: x_factor = [0.5, 0.5, -1.0] y_factor = [1.1547, -1.1547, 0.0] theta_factor = [-1.0, -1.0, -1.034863] trace('calling move_robot') self._robot_speed_x = float(x_velocity) self._robot_speed_y = float(y_velocity) self._robot_speed_theta = float(theta_velocity) trace('move robot = %6.2f, %6.2f, %6.2f', x_velocity, y_velocity, theta_velocity) x_vel = self._robot_speed_x * cMotors.meter_per_sec_to_encoder_value y_vel = self._robot_speed_y * cMotors.meter_per_sec_to_encoder_value theta_vel = self._robot_speed_theta * cMotors.rad_per_sec_to_encoder_value left_motor_velocity = x_vel * x_factor[0] + y_vel * y_factor[ 0] + theta_vel * theta_factor[0] right_motor_velocity = x_vel * x_factor[1] + y_vel * y_factor[ 1] + theta_vel * theta_factor[1] rear_motor_velocity = x_vel * x_factor[2] + y_vel * y_factor[ 2] + theta_vel * theta_factor[2] self._set_speed(left_motor_velocity, right_motor_velocity, rear_motor_velocity) 'Update coupled Compass simulator' self._compass._update_angle_speed(theta_vel) 'Update ball possession' self._trigger_event_on_ball_possession_change() 'Update encoder readings' self._read_driving_encoders() trace('robot moved') except: traceError('Cannot move robot')
def __init__(self): ''' Initialize Kicker class Steps: - Open serial ports for I/O board ''' self._shoot_height = 0.0 self._shoot_speed = 0.0 try: trace('Waiting for shoot to come online') rospy.wait_for_service('/simulator/shoot') self._sim_shoot_srv = rospy.ServiceProxy('/simulator/shoot', Shoot, True) trace('robot sim ball online') rospy.wait_for_service("s_set_own_robot_status") self._set_robot_status_srv = rospy.ServiceProxy( "s_set_own_robot_status", set_own_robot_status, True) trace('Set own robot status service online.') self._srv_setKickPosition = rospy.Service( 's_kick_position', s_peripheralsInterface_setKickPosition, self.setKickPosition) self._srv_setKickSpeed = rospy.Service( 's_kick_speed', s_peripheralsInterface_setKickSpeed, self.setKickSpeed) # Set robot in play robot_status_req = set_own_robot_statusRequest() robot_status_req.robotStatus.status_type = rosMsgs.msg.s_robot_status.TYPE_INPLAY _ = self._set_robot_status_srv(robot_status_req) except Exception as e: traceError('failed to initialize cKicker') print traceback.format_exc() print sys.exc_info()[0] raise (e)
def _update_angle_speed(self, vel_theta): try: self._calc_angle() self._vel_angle = vel_theta except: traceError('Failed to update angle speed')