class Controller(object):
def __init__(self, swarm_prefix='/swarm/proxy/'):
self.swarm_prefix = swarm_prefix
self.last_input = GloveInput()
self.flying = False
self.current_trajectory = None
self.POSSIBLE_TRAJECTORIES = [
ControlModeHover,
ControlModeJuggle3D,
ControlModeCircle,
ControlModeBTimed,
]
self.ball_first_pos_nparr = None
self.manager = PubSubManager('juggling_controller',
anonymous=False,
log_level=rospy.DEBUG)
self.nb_drones = int(self.manager.get_param('~nb_drones', 2))
self.controller_id = int(self.manager.get_param('~controller_id', 0))
self.selected_drone = min(self.controller_id, self.nb_drones - 1)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/load_trajectory',
Proxy_LoadTrajectory)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/start_trajectory', Proxy_Empty)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/stop_trajectory', Proxy_Empty)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/visualize',
Proxy_VisualizeTrajectory)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/next_trajectory', Proxy_Empty)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/trajectory/config_bool',
Proxy_ConfigTrajectoryBool)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/trajectory/config_float',
Proxy_ConfigTrajectoryFloat)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/trajectory/config_string',
Proxy_ConfigTrajectoryString)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/trajectory/config_vector',
Proxy_ConfigTrajectoryVector)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/takeoff', Proxy_Empty)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/land', Proxy_Empty)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/translate_trajectory',
Proxy_TranslateTrajectory)
self.manager.add_client_service(
swarm_prefix + 'trajectory_manager/set_trajectory_translation',
Proxy_TranslateTrajectory)
self.manager.add_client_service(swarm_prefix + 'emergency',
Proxy_Empty)
self.manager.add_subscriber('input', GloveInput, self._on_glove_input)
def _on_glove_input(self, input_wrapper):
glove = input_wrapper['data']
if glove.glove_id.data != self.controller_id:
return
# compute differences from buttons
button_diff = [
int(_a) - int(_b)
for _a, _b in zip(glove.buttons, self.last_input.buttons)
]
buttons = {}
buttons['select_button'] = button_diff[0] # l/r 1
buttons['action_buttons'] = button_diff[1:5]
buttons['takeoff_land_button'] = button_diff[5]
buttons['translate'] = button_diff[6] # l/r 3
buttons['emergency'] = button_diff[7] # joystick up
buttons['prev_drone'] = button_diff[8] # joystick left
buttons['next_drone'] = button_diff[9] # joystick right
print buttons
if buttons['prev_drone'] == 1:
self._prev_drone()
print 'SELECTED DRONE :', self.selected_drone
if buttons['next_drone'] == 1:
self._next_drone()
print 'SELECTED DRONE :', self.selected_drone
if buttons['emergency'] == 1:
self._call_service('emergency')
# land/takeoff
if buttons['takeoff_land_button'] == 1:
if self.flying:
self._do_land()
else:
self._do_takeoff()
# trajectory selection
if 1 in buttons['action_buttons']:
print 'TRAJECTORY SELECTION'
trajectory_selected = self.POSSIBLE_TRAJECTORIES[
buttons['action_buttons'].index(1)]
print self._call_service(
'trajectory_manager/load_trajectory',
tid=1,
trajectory_type=trajectory_selected.get_name())
self.current_trajectory = trajectory_selected(self)
print 'TRAJECTORY SELECTED :', self.current_trajectory.get_name()
if buttons['translate'] == -1:
vel = vec3_to_tuple(glove.velocity)
self._call_service('trajectory_manager/translate_trajectory',
translation=vel)
self._call_service('trajectory_manager/visualize', tid=0)
# trajectory configuration
if self.current_trajectory is not None:
self.current_trajectory.on_input(glove, buttons)
self.last_input = glove
def _next_trajectory(self):
self._call_service('trajectory_manager/next_trajectory')
self._call_service('trajectory_manager/start_trajectory')
self.current_trajectory = None
self.ball_first_pos_nparr = None
def _call_service(self, service, **kwargs):
print 'CALLING SERVICE', service, kwargs
if len(kwargs.keys()) > 0:
res = self.manager.call_service(self.swarm_prefix + service,
drone_id=self.selected_drone,
payload=objectview(kwargs))
else:
res = self.manager.call_service(self.swarm_prefix + service,
drone_id=self.selected_drone)
print 'SERVICE RESULT :', res
return res
def _do_land(self):
if self.flying:
self._call_service('trajectory_manager/land')
self.flying = False
def _do_takeoff(self):
if not self.flying:
self._call_service('trajectory_manager/takeoff')
self.flying = True
def _prev_drone(self):
self.selected_drone = (self.selected_drone + self.nb_drones -
1) % self.nb_drones
def _next_drone(self):
self.selected_drone = (self.selected_drone + 1) % self.nb_drones
class HydraMapper(object):
def __init__(self):
self.manager = PubSubManager('hydra_mapper', anonymous=False)
self.INPUT_ID = int(self.manager.get_param('~input_id', 0))
self.positions = []
self.COLOR = [ColorRGBA(0, 255, 0, 255), ColorRGBA(0, 0, 255, 255)]
self.manager.add_publisher('viz', Marker)
self.manager.add_publisher('input', GloveInput)
self.manager.add_subscriber('/hydra_calib', Hydra, self.onHydra)
self.last_message_sent_millis = 0
self.framerate = 30
def fixed_paddle(self, paddle):
new_paddle = copy.deepcopy(paddle)
new_paddle.transform.translation = \
v3_switch_x_y(paddle.transform.translation)
new_paddle.transform.translation.x += 1.75
new_paddle.transform.translation.y += 2.25
new_paddle.transform.translation.z += 1.5
return new_paddle
def paddle_to_marker(self, paddle, color, _id):
m = Marker()
m.pose.position = paddle.transform.translation
# m.pose.orientation = Vector3() #paddle.transform.rotation
m.scale = Vector3(0.1, 0.1, 0.1)
m.color = color
m.header = std_msgs.msg.Header()
m.header.stamp = self.manager.rospy.Time.now()
m.header.frame_id = '/world'
m.type = 1
m.id = _id
m.lifetime = self.manager.rospy.Duration(10)
return m
def create_glove(self, position, rotation, velocity, acceleration, buttons,
trigger, joy):
glove = GloveInput()
glove.glove_id = Int32(self.INPUT_ID)
glove.position = Vector3(*position)
glove.velocity = Vector3(*velocity)
glove.acceleration = Vector3(*acceleration)
glove.rotation = Quaternion(*rotation)
glove.buttons = (
buttons +
# [trigger > 0.5] +
[joy[1] > 0.75] + [joy[0] < -0.75] + [joy[0] > 0.75])
return glove
def onHydra(self, hydra_wrapper):
h = hydra_wrapper['data']
paddle = self.fixed_paddle(h.paddles[self.INPUT_ID])
if self.last_message_sent_millis is 0:
self.last_message_sent_millis = millis()
# retrieve positions
self.positions.append(
np.array(vec3_to_tuple(paddle.transform.translation)))
# should send a new message ?
if millis() - self.last_message_sent_millis >= 1000.0 / self.framerate:
delta_time = float(millis() - self.last_message_sent_millis) / 1000
position = np.array(vec3_to_tuple(paddle.transform.translation))
rotation = quat_to_tuple(paddle.transform.rotation)
# computes average velocity since last message
velocities = [
_a - _b for _a, _b in zip(self.positions[1:], self.positions)
]
if len(velocities) == 0:
velocities.append(np.array([0, 0, 0]))
velocities = [
v / (delta_time / len(velocities)) for v in velocities
]
velocity = sum(velocities) / len(velocities)
accelerations = [
_a - _b for _a, _b in zip(velocities[1:], velocities)
]
if len(accelerations) == 0:
accelerations.append(np.array([0, 0, 0]))
accelerations = [
a / (delta_time / len(accelerations)) for a in accelerations
]
acceleration = sum(accelerations) / len(accelerations)
self.last_message_sent_millis = millis()
self.positions = []
marker = self.paddle_to_marker(paddle, self.COLOR[self.INPUT_ID],
0)
self.manager.publish('viz', marker)
self.manager.publish(
'input',
self.create_glove(tuple(position), rotation, tuple(velocity),
tuple(acceleration), paddle.buttons,
paddle.trigger, paddle.joy))