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)