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 TrajectoryManager(object):
def __init__(self, bbox=TrajectoryBBOX()):
self.listening = False
self.manager = PubSubManager('trajectory_manager', anonymous=False)
self.manager.add_publisher('goal', PoseStamped)
self.manager.add_publisher('trajectory/viz/line', Marker, latch=True)
self.manager.add_publisher('trajectory/viz/points', Marker, latch=True)
self.manager.add_publisher('trajectory/viz/bbox', Marker, latch=True)
try:
self.manager.add_server_service('trajectory_manager/load_trajectory',
LoadTrajectory, self.srv_load_trajectory)
self.manager.add_server_service('trajectory_manager/start_trajectory',
Empty, self.srv_start_trajectory)
self.manager.add_server_service('trajectory_manager/stop_trajectory',
Empty, self.srv_stop_trajectory)
self.manager.add_server_service('trajectory_manager/visualize',
VisualizeTrajectory, self.srv_visualize)
self.manager.add_server_service('trajectory_manager/next_trajectory',
Empty, self.srv_next_trajectory)
self.manager.add_server_service('trajectory_manager/trajectory/config_bool',
ConfigTrajectoryBool, self.srv_config_trajectory)
self.manager.add_server_service('trajectory_manager/trajectory/config_float',
ConfigTrajectoryFloat, self.srv_config_trajectory)
self.manager.add_server_service('trajectory_manager/trajectory/config_string',
ConfigTrajectoryString, self.srv_config_trajectory)
self.manager.add_server_service('trajectory_manager/trajectory/config_vector',
ConfigTrajectoryVector, self.srv_config_trajectory)
self.manager.add_server_service('trajectory_manager/takeoff',
Empty, self.srv_takeoff)
self.manager.add_server_service('trajectory_manager/land',
Empty, self.srv_land)
self.manager.add_server_service('trajectory_manager/translate_trajectory',
TranslateTrajectory, self.srv_translate_trajectory)
self.manager.add_server_service('trajectory_manager/set_trajectory_translation',
TranslateTrajectory, self.srv_set_trajectory_translation)
self.manager.add_server_service('ready', Empty, self.srv_nop)
except Exception as e:
print 'ERROR WHILE CREATING SERVICES :', e
self.manager.add_client_service('takeoff', Empty)
self.manager.add_client_service('land', Empty)
self.bbox = bbox
self.trajectories = [None, None]
self.drone_position = None
self.trajectory_translation = (0,0,0)
# ENUM
self.flight_state = FLIGHT_STATES.LANDED
def visualize_trajectory(self, tid=0):
vline, vpoints = self.trajectories[tid].visualize()
line = Marker()
line.scale = Vector3(0.1, 0.1, 0.1)
line.color = ColorRGBA(1, 0, 1, 1)
line.header = std_msgs.msg.Header()
line.header.stamp = self.manager.rospy.Time.now()
line.header.frame_id = '/world'
line.type = 4
line.id = 0xF00D
line.lifetime = self.manager.rospy.Duration(0)
# bbox cropping
line.points = [Point(*p) for p in self.bbox.crop_trajectory(vline)]
line.colors = [ColorRGBA(1, 0, float(idx) / len(line.points), 1) for idx, p in enumerate(line.points)]
self.manager.publish('trajectory/viz/line', line)
pts = Marker()
pts.scale = Vector3(0.1, 0.1, 0.1)
pts.color = ColorRGBA(1, 1, 0, 1)
pts.header = std_msgs.msg.Header()
pts.header.stamp = self.manager.rospy.Time.now()
pts.header.frame_id = '/world'
pts.type = 8
pts.id = 0xF00D + 1
pts.lifetime = self.manager.rospy.Duration(0)
pts.points = [Point(*p) for p in vpoints]
pts.colors = [ColorRGBA(1, float(idx) / len(pts.points), 0, 1) for idx, p in enumerate(pts.points)]
self.manager.publish('trajectory/viz/points', pts)
def visualize_bbox(self):
bbox = Marker()
bbox.header = std_msgs.msg.Header()
bbox.header.stamp = self.manager.rospy.Time.now()
bbox.header.frame_id = '/world'
bbox.type=1
bbox.color = ColorRGBA(1, 0, 0, 0.2)
def mean((a, b)):
return float(a + b) / 2
bbox.pose.position = Vector3(*map(mean, (self.bbox.x, self.bbox.y, self.bbox.z)))
bbox.scale = Vector3(*map(lambda (a,b): min([100, b-a]), (self.bbox.x, self.bbox.y, self.bbox.z)))
self.manager.publish('trajectory/viz/bbox', bbox)
def load_trajectory(self, traj, tid=0):
assert issubclass(traj.__class__, Trajectory) == True
self.trajectories[tid] = traj
def start(self):
if self.listening == False:
self.manager.add_subscriber('pose', PoseStamped, self._on_pose)
self.listening = True
def _on_pose(self, poseWrapper):
self.drone_position = vec3_to_tuple(poseWrapper['data'].pose.position)
if self.trajectories[0] and self.trajectories[0].started:
pose = poseWrapper['data']
ps = PoseStamped()
ps.header = std_msgs.msg.Header()
ps.header.stamp = self.manager.rospy.Time.now()
ps.header.frame_id = '/world'
ps.pose = self.trajectories[0].get_next_pose(self.drone_position)
# bbox cropping
ps.pose.position = Vector3(*self.bbox.crop_point(vec3_to_tuple(ps.pose.position)))
self.manager.publish('goal', ps)
# Services controllers
def trajectory_start(self):
if self.trajectories[0]:
self.trajectories[0].start()
def trajectory_stop(self):
if self.trajectories[0]:
self.trajectories[0].stop()
def trajectory_config(self, tid, **kwargs):
if self.trajectories[tid]:
for k in kwargs:
self.trajectories[tid][k] = kwargs[k]
def takeoff(self):
if self.flight_state == FLIGHT_STATES.LANDED:
# if there is nothing to do for the drone, make it hover at 1.5 meters from the ground
if self.trajectories[0] is None and self.drone_position is not None:
self.load_trajectory(HoverTrajectory(manager=self), tid=0)
pos = (self.drone_position[0], self.drone_position[1], 1.5)
self.trajectory_config(tid=0, position=pos)
self.trajectory_start()
self.manager.call_service('takeoff')
self.flight_state = FLIGHT_STATES.FLYING
def land(self):
if self.flight_state == FLIGHT_STATES.FLYING:
self.manager.call_service('land')
self.flight_state = FLIGHT_STATES.LANDED
def next_trajectory(self):
self.trajectories = self.trajectories[1:] + [None]
if self.trajectories[0] is None:
self.translate_trajectory([-p for p in self.trajectory_translation])
def translate_trajectory(self, translation):
self.trajectory_translation = tuple3_add(self.trajectory_translation, translation)
for t in self.trajectories:
if t is not None:
t.set_translation(self.trajectory_translation)
return self.trajectory_translation
# Services views
@service
def srv_load_trajectory(self, req, res):
print 'LOADING TRAJECTORY', req.payload.tid, req.payload.trajectory_type
TRAJS = {
'hover': HoverTrajectory,
'bspline': BSplineTrajectory,
'btimed': BSplineTimedTrajectory,
'ball': BallLikeTrajectory,
'grav': GravitationTrajectory,
'land_at_pos': LandTrajectory,
'juggle3D': Juggle3DTrajectory
}
if req.payload.trajectory_type in TRAJS:
t = TRAJS[req.payload.trajectory_type](manager=self)
print t.items()
self.load_trajectory(t, req.payload.tid)
res.result = True
else:
res.result = False
return res
@service
def srv_start_trajectory(self, req, res):
print 'STARTING TRAJECTORY'
self.trajectory_start()
return res
@service
def srv_stop_trajectory(self, req, res):
print 'STOPPING TRAJECTORY'
self.trajectory_stop()
return res
@service
def srv_config_trajectory(self, req, res):
print 'CONFIGURING TRAJECTORY', req.payload.tid, req.payload.key, '=', req.payload.value
if self.trajectories[req.payload.tid] is not None:
self.trajectory_config(req.payload.tid, **{req.payload.key: req.payload.value})
res.result = True
else:
res.result = False
return res
@service
def srv_visualize(self, req, res):
if self.trajectories[req.payload.tid] is not None:
self.visualize_trajectory(req.payload.tid)
res.result = True
else:
res.result = False
self.visualize_bbox()
return res
@service
def srv_takeoff(self, req, res):
self.takeoff()
return res
@service
def srv_land(self, req, res):
self.land()
return res
@service
def srv_next_trajectory(self, req, res):
self.next_trajectory()
return res
@service
def srv_translate_trajectory(self, req, res):
t = self.translate_trajectory(req.payload.translation)
res.result = t
return res
@service
def srv_set_trajectory_translation(self, req, res):
to_translate = (
req.payload.translation[0] - self.trajectory_translation[0],
req.payload.translation[1] - self.trajectory_translation[1],
req.payload.translation[2] - self.trajectory_translation[2]
)
t = self.translate_trajectory(to_translate)
res.result = t
return res
@service
def srv_nop(self, req, res):
return res
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))