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