def measurement_check(self):
'''Monitor function: checks measurement.
If the measurement equals the vive frame origin, this means that
vibrations cause a false measurement.
Should be moved to monitor later.
'''
tf_v_in_w = self.get_transform("vive", "world")
tf_t_in_w = self.get_transform("tracker", "world")
meas_distance = np.linalg.norm(
np.array([
tf_t_in_w.transform.translation.x, tf_t_in_w.transform.
translation.y, tf_t_in_w.transform.translation.z
]) - np.array([
self.tf_t_in_w_prev.transform.translation.x,
self.tf_t_in_w_prev.transform.translation.y,
self.tf_t_in_w_prev.transform.translation.z
]))
self.tf_t_in_w_prev = tf_t_in_w
measurement_valid = not ((tf_v_in_w.transform == tf_t_in_w.transform)
or (meas_distance > 0.25))
if not measurement_valid:
if not (self.init or self.vive_calibrating):
print highlight_red(' Warning: invalid measurement!')
else:
self.init = False
self.vive_calibrating = False
return measurement_valid
def switch_task(self, task):
'''Reads out the task topic and switches to the desired task.
'''
if task.data not in self.task_dict:
print highlight_red(
' Not a valid task, drone will remain in standby state.')
self.state_sequence = self.task_dict.get(task.data, [])
self.new_task = True
print cyan(' Core received a new task: ', task.data)
def _default_output(cls, entries: list, limit: int, top_data_output: Map,
is_colorize) -> str:
"""
This method render data for default output case
:param entries: list
:param limit: int
:param top_data_output: Map
:param is_colorize: bool
:return:
"""
if is_colorize:
print(
Text("Console Rss Reader!",
fsize=19,
color='#f44a41',
shadow=False,
skew=4))
formatted_feeds = ''.join(
cls._colorize_single_feed_format_default(feed)
for feed in entries[:limit])
else:
formatted_feeds = ''.join(
cls._single_feed_format_default(feed)
for feed in entries[:limit])
if is_colorize:
return 'Feed: {0}\nUrl: {1}\n\n{2}'.format(
color.highlight_black(top_data_output.title),
color.highlight_red(top_data_output.url), formatted_feeds)
return 'Feed: {0}\nUrl: {1}\n\n{2}'.format(
f'——— {top_data_output.title} ———',
f'——— {top_data_output.url} ———', formatted_feeds)
def start(self):
'''
Starts running of localization node.
'''
self.rate = rospy.Rate(1. / self.sample_time)
self.v = triad_openvr.triad_openvr()
# self.v.print_discovered_objects()
if not self.v.devices:
print highlight_red('! Vive Error: No devices found !')
return
self.init_transforms()
self.publish_pose_est()
rospy.spin()
def switch_task(self, task):
'''Reads out the task topic and switches to the desired task.
'''
if task.data in [
"home inspection", "home count", "home inventory",
"home guide", "home picture", "home events",
"home smart inspection"
]:
self.task_str = task.data
else:
if task.data not in self.task_dict:
print highlight_red(
' Not a valid task, drone will remain in standby state.')
self.state_sequence = self.task_dict.get(task.data, [])
self.new_task = True
print cyan(' Bebop_core received a new task: ', task.data)
def events(self):
'''This function enables the actions on obstacles and or stoplight
'''
if [15] in self.ids and not self.obstacle_1:
print magenta("Watch out! Unknown obstacle ahead!")
self.obstacle_1 = True
elif self.obstacle_1 and [15] not in self.ids:
self.obstacle_1 = False
if [18] in self.ids:
print magenta(
"Watch out! Dynamic obstacle ahead! Please land and wait for 10 seconds"
)
rospy.sleep(10)
print magenta("You are allowed to start again")
while [14] in self.ids and [13] not in self.ids:
print highlight_red('The light is red! You can not pass.')
rospy.sleep(2)
while [13] in self.ids:
print highlight_green('The light is green! you can proceed.')
rospy.sleep(2)
def update(self, cmd):
"""Updates the motionplanner and the deployer solving the Point2point
problem. Publishes trajectories resulting from calculations.
Args:
cmd : contains data sent over Trigger topic.
"""
# In case goal has changed: set new goal.
if cmd.goal_pos != self._goal:
self._goal = cmd.goal_pos
self._vehicle.set_initial_conditions(
[
cmd.pos_state.position.x, cmd.pos_state.position.y,
cmd.pos_state.position.z
],
# [cmd.vel_state.x, cmd.vel_state.y, cmd.vel_state.z]
)
self._vehicle.set_terminal_conditions([
self._goal.position.x, self._goal.position.y,
self._goal.position.z
])
self._deployer.reset()
print magenta('---- Motionplanner received a new goal -'
' deployer resetted ----')
state0 = [
cmd.pos_state.position.x, cmd.pos_state.position.y,
cmd.pos_state.position.z
]
input0 = [cmd.vel_state.x, cmd.vel_state.y, cmd.vel_state.z]
for k in range(self.n_dyn_obst):
pos = cmd.dyn_obstacles[k].pose
vel = cmd.dyn_obstacles[k].velocity
# Dirty fix necessary to make dynamic obstacle work in OMG-tools.
# -> NIET OKE, gedverdekke Ruben.
pos = np.round(pos, 1)
vel = np.round(vel, 1)
obst_i = k + self.n_stat_obst
(self._deployer.problem.environment.obstacles[obst_i].set_state({
'position':
pos,
'velocity':
vel
}))
trajectories = self._deployer.update(cmd.current_time,
state0) # input0)
calc_succeeded = True
return_status = self._deployer.problem.problem.stats()['return_status']
if (return_status != 'Solve_Succeeded'):
print highlight_red(return_status, ' -- brake! ')
calc_succeeded = False
self._result = Trajectories(u_traj=trajectories['input'][0, :],
v_traj=trajectories['input'][1, :],
w_traj=trajectories['input'][2, :],
x_traj=trajectories['state'][0, :],
y_traj=trajectories['state'][1, :],
z_traj=trajectories['state'][2, :],
success=calc_succeeded)
print(str(max(trajectories['input'][0, :])))
print(str(max(trajectories['input'][1, :])))
print(str(max(trajectories['input'][2, :])))
self._mp_result_topic.publish(self._result)
def log(self):
if self.passed:
print highlight_green(self.display_str)
else:
print highlight_red(self.display_str)